/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/kinect_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 KINECT_CLOUD_H
#define KINECT_CLOUD_H

#include <string>
#include <vector>

#include "cloud.h"
#include "options.h"
#include "math_types.h"

#include "ntk/camera/rgbd_processor.h"
#include "ntk/camera/calibration.h"
#include "ntk/mesh/mesh_generator.h"
#include <boost/filesystem.hpp>

class kinect_cloud : public cloud3
{
  public:
    kinect_cloud (const options &opts, const calibration &_calib)
      : cloud3 (opts, _calib),
        input_template (opts.data_template ())
      {
        mesh.setUseColor(true);
      
        const std::string calib_file = opts.kinect_calibration ();
        kinect_calibration.loadFromFile (calib_file.c_str ());

        // Tell the processor to transform raw depth into meters using linear coefficients.
        processor.setFilterFlag (ntk::RGBDProcessor::ComputeKinectDepthBaseline, true);
        processor.setFilterFlag (ntk::RGBDProcessor::FilterMedian, true);
        processor.setFilterFlag (ntk::RGBDProcessor::ComputeNormals, true);
        processor.setFilterFlag (ntk::RGBDProcessor::SmoothDepth, true);
        
        init ();
      }
    
    virtual bool read_background (const std::string &depth_background)
      {
        point_vector P;
        const int num_frames = 5;
        P.reserve (num_frames * 640 * 480);
        bg_normals.reserve (num_frames * 640 * 480);
        for (int bg_frame = 15; bg_frame < 15+num_frames; bg_frame++)
          {
            const char *format = "%s/view%.4d";
            char buffer [100];
            sprintf (buffer, format, depth_background.c_str (), bg_frame);
                
            if (boost::filesystem::is_directory (buffer))
              {
                current_frame.loadFromDir (buffer, &kinect_calibration);
                processor.processImage (current_frame);
                mesh.generate (current_frame);
                const cv::Mat3f &mesh_normals = current_frame.normal ();
                    
                // Copy the data
                const size_t num_points = mesh.mesh ().vertices.size ();                      
                for (size_t k = 0; k < num_points; k++)
                  {
                    const cv::Point3f xyz = mesh.mesh ().vertices [k];
              
                    const double s = 1000.0;
                    const vector3 point = calib.camera_to_world (s*vector3 (xyz.x, xyz.y, -xyz.z));
                    P.push_back (point);
                        
                    const int row = k / mesh_normals.cols, col = k - row * mesh_normals.cols;
                    const cv::Vec3f normal = mesh_normals (row, col);

                    const vector3 tmp = calib.camera_to_world (s*vector3 (normal [0] + xyz.x, normal [1] + xyz.y, -normal [2] - xyz.z)) - point;
                    bg_normals.push_back (unit (tmp));
                  }
              }
            else
              break;
          }

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


    bool refresh (bool goto_next = true)
      {
        // Read data from disk
        char dirname [100];
        sprintf (dirname, "%s/view%.4d", input_template.c_str (), frame);
        if (goto_next) frame ++;
        
        const bool found = boost::filesystem::is_directory (dirname);
        if (!found)
          {
            std::cerr << "kinect_cloud::refresh: could not open '"
                      << dirname << "' for reading" << std::endl;
          }
        else
          {
            // Read data into a mesh
            const std::string sdirname (dirname);
            current_frame.loadFromDir (sdirname, &kinect_calibration);
            processor.processImage (current_frame);
            mesh.generate (current_frame);
          
            // Allocate data storage
            const size_t num_points = mesh.mesh ().vertices.size ();
            points.reserve (num_points / downsample);
            normals.reserve (num_points / downsample);
            colours.reserve (num_points / downsample);
  
            // Copy the data
            const std::vector<cv::Point3f> &vertices = mesh.mesh ().vertices;
            const cv::Mat3f &mesh_normals = current_frame.normal ();
            const std::vector<cv::Vec3b> &colors = mesh.mesh ().colors;
            for (size_t k = 0; k < num_points; k += downsample)
              {
                const cv::Point3f xyz = vertices [k];
                const int row = k / mesh_normals.cols, col = k - row * mesh_normals.cols;
                const cv::Vec3f normal = mesh_normals (row, col);
                const cv::Vec3b rgb = colors [k];
                
                const double s = 1000.0;
                const vector3 point = calib.camera_to_world (s*vector3 (xyz.x, xyz.y, -xyz.z));
                
                if (!use_background_subtraction || (point (1) > 1.75 && bg_nearest_cloud.nearest_distance (point) > depth_threshold && norm2 (point) < 200.0))
                  {
                    points.push_back (point);
                    const vector3 tmp = calib.camera_to_world (s*vector3 (normal [0] + xyz.x, normal [1] + xyz.y, -normal [2] - xyz.z)) - point;
                    normals.push_back (unit (tmp));
                    colours.push_back (vector3 (rgb [0], rgb [1], rgb [2]) / 255.0);
                  }
              }
      
            // Render background
//            render_background_image (background, PROJ2D_SCALE);
//            proj2d.update_segment (background);
            
            // Update kNN structure
            nearest_cloud.refresh (points);
          }

        return found;
      }
    
    vector3 normal (const size_t i) const
      {
        if (i < normals.size ())
          return normals [i];
        else
          return vector3 (0, 0, 0);
      };
    
    vector3 bg_normal (const size_t i) const
      {
        if (i < bg_normals.size ())
          return bg_normals [i];
        else
          return vector3 (0, 0, 0);
      }
    
    void show () const
      {
        cloud3::show ();
        
        glDisable (GL_LIGHTING);
        glBegin (GL_LINES);
        glColor3f (0.2, 0.2, 0.8);
        for (size_t k = 0; k < points.size (); k+=20)
          {
            const vector3 p = points [k];
            const vector3 normal = p + normals [k];
            glVertex3f (p [0], p [1], p [2]);
            glVertex3f (normal [0], normal [1], normal [2]);
          }
        glEnd ();
        glEnable (GL_LIGHTING);
      }

  protected:
    const std::string input_template;
    ntk::RGBDCalibration kinect_calibration;
    ntk::NiteProcessor processor;
    ntk::RGBDImage current_frame;
    ntk::MeshGenerator mesh;
    point_vector normals, bg_normals;
};

#endif // KINECT_CLOUD_H