/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/point_cloud.h

Go to the documentation of this file.

// Copyright (C) 2011 Soren Hauberg
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, see <http://www.gnu.org/licenses/>.
 
#ifndef POINT_CLOUD_H
#define POINT_CLOUD_H

#include <string>
#include <vector>

#include "cloud.h"
#include "options.h"
#include "math_types.h"
#include <boost/filesystem.hpp>

class point_cloud : public cloud3
{
  public:
    point_cloud (const options &opts, const calibration &_calib)
      : cloud3 (opts, _calib),
        input_template (opts.data_template ())
      {
        init ();
      };
    
    virtual bool read_background (const std::string &depth_background)
      {
        point_vector P;
        const int num_frames = 5;
        P.reserve (num_frames * 320 * 240);
        for (int bg_frame = 15; bg_frame < 15+num_frames; bg_frame++)
          {
            const char *format = "%s/points_%d.txt";
            char buffer [100];
            sprintf (buffer, format, depth_background.c_str (), bg_frame);
                
            if (boost::filesystem::is_regular (buffer))
              {
                FILE *fid = fopen (buffer, "r");
                const bool found = (fid != NULL);
                if (!found)
                  {
                    std::cerr << "point_cloud::point_cloud: could not open depth background '"
                              << buffer << "' for reading" << std::endl;
                  }
                else
                  {
                    // Count number of lines in file
                    const int buf_size = 100;
                    char line [buf_size];
                    size_t num_points = 0;
                    while (fgets (line, buf_size, fid) != NULL)
                      num_points ++;
  
                    // Read the data
                    rewind (fid);
                    for (size_t k = 0; k < num_points; k++)
                      {
                        double x, y, z;
                        double r, g, b;
                        const int num_read = fscanf (fid, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b);
                        if (num_read != 6)
                          {
                            std::cerr << "point_cloud::point_cloud: invalid data format" << std::endl;
                            break;
                          }
                
                        const vector3 point = calib.camera_to_world (vector3 (x, y, z));
                        P.push_back (point);
                      }
                  }

                fclose (fid);
              }
          }

        if (P.size () > 0)
          bg_nearest_cloud.refresh (P);
        
        return (P.size () > 0);
      }

    bool refresh (bool goto_next = true)
      {
        // Read data from disk
        char filename [100];
        sprintf (filename, input_template.c_str (), frame);
        if (goto_next) frame ++;
        
        FILE *fid = fopen (filename, "r");
        const bool found = (fid != NULL);
        if (!found)
          {
            std::cerr << "point_cloud::refresh: could not open '"
                      << filename << "' for reading" << std::endl;
          }
        else
          {
            // Count number of lines in file
            const int buf_size = 100;
            char line [buf_size];
            size_t num_points = 0;
            while (fgets (line, buf_size, fid) != NULL)
              num_points ++;

            // Allocate data storage
            points.clear ();
            points.reserve (num_points / downsample);
            colours.clear ();
            colours.reserve (num_points / downsample);
  
            // Read the data
            rewind (fid);
            const double y_thresh = -0.5; // For hands2
            for (size_t k = 0; k < num_points; k++)
              {
                double x, y, z;
                double r, g, b;
                const int num_read = fscanf (fid, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b);
                if (num_read != 6)
                  {
                    std::cerr << "point_cloud::refresh: invalid data format" << std::endl;
                    break;
                  }
                
                // Should we downsample the points?
                if (k % downsample == 0)
                  {
                    const vector3 point = calib.camera_to_world (vector3 (x, y, z));
                
                    if (!use_depth_threshold || (point [2] > depth_threshold && point [1] > y_thresh)) 
                      {
                        double d = -1;
                        if (!use_background_subtraction || (d = bg_nearest_cloud.nearest_distance (point)) > 0.5)
                          {
                            points.push_back (point);
                            colours.push_back (vector3 (r, g, b));
                          }
                      }
                  }
              }
            fclose (fid);
            // Render background
            //render_background_image (background, PROJ2D_SCALE);
            //proj2d.update_segment (background);
            
            // Update kNN structure
            nearest_cloud.refresh (points);
          }

        return found;
      }

    vector3 bg_normal (const size_t i) const
      {
        return vector3 (0, 0, 0);
      }

  protected:
    const std::string input_template;
};

#endif // POINT_CLOUD_H