openni_range_image_registration_reader.cpp

Go to the documentation of this file.

/***************************************************************************
 *            openni_range_image_registration_reader.cpp
 *
 *  Wed 13 Jul 2011 04:16:30 PM CEST
 *  Copyright 2011 Rainer Kümmerle
 *  Email rk@raikue.net
 ****************************************************************************/
/* \author Bastian Steder */

#include <set>
#include <map>
#include <iostream>
using namespace std;
#include "pcl/range_image/range_image_planar.h"
#include "pcl/common/common_headers.h"
#include "pcl/visualization/range_image_visualizer.h"
#include "pcl/visualization/pcl_visualizer.h"
#include "pcl/console/parse.h"
#include <boost/filesystem.hpp> 
#include "pcl/io/pcd_io.h"
#include <boost/thread/thread.hpp>
//#include "boost/filesystem/operations.hpp"
//#include "boost/filesystem/path.hpp"

using namespace pcl;
using namespace pcl::visualization;

std::string dir_name = ".";

float angular_resolution = -1;

int icp_search_radius_consecutive = 3;
float icp_max_distance_start_consecutive=0.3, icp_max_distance_end_consecutive=0.05;
int icp_num_iterations_consecutive = 20;
int icp_search_radius_keyframe = 1;
float icp_max_distance_start_keyframe=0.15, icp_max_distance_end_keyframe=0.05;
int icp_num_iterations_keyframe = 10;
float overlap_calculation_max_error = 0.25;
float new_keyframe_overlap = 0.85;
int max_no_of_threads = 1;
float output_resolution = 0.005;

float maximum_range = -1.0f;

struct VoxelGridCloud {
  VoxelGridCloud(float resolution=0.01) : resolution(resolution) {}
  template <typename PointType>
  void addPoint(const PointType& point)
  {
    unsigned int cell_x = lrintf(point.x/resolution) + (std::numeric_limits<unsigned int>::max()/2),
                 cell_y = lrintf(point.y/resolution) + (std::numeric_limits<unsigned int>::max()/2),
                 cell_z = lrintf(point.z/resolution) + (std::numeric_limits<unsigned int>::max()/2);
    voxels[cell_x][cell_y].insert(cell_z);
  }
  void getPointCloud(PointCloud<PointXYZ>& cloud) const
  {
    for (std::map<unsigned int, std::map<unsigned int, std::set<unsigned int> > >::const_iterator it_x=voxels.begin();
         it_x!=voxels.end(); ++it_x)
    {
      float x = (double(it_x->first)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
      for (std::map<unsigned int, std::set<unsigned int> >::const_iterator it_y=it_x->second.begin();
           it_y!=it_x->second.end(); ++it_y)
      {
        float y = (double(it_y->first)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
        for (std::set<unsigned int>::const_iterator it_z=it_y->second.begin();
             it_z!=it_y->second.end(); ++it_z)
        {
          float z = (double(*it_z)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
          cloud.points.push_back(PointXYZ(x, y, z));
        }
      }
    }
  }
  std::map<unsigned int, std::map<unsigned int, std::set<unsigned int> > > voxels;
  float resolution;
};

bool show_color_image = 1,
     show_range_image = 1,
     show_registered_clouds = 1;

Eigen::Affine3f last_pose, current_pose=Eigen::Affine3f::Identity();

std::vector<std::string> readInfoFiles(const std::string& folder_name)
{
  std::vector<std::string> file_names;
  boost::filesystem::path folder( folder_name );
  if ( !boost::filesystem::exists( folder ) )
  {
    std::cerr << "Folder \""<<dir_name<<"\" does not exist.\n";
    return file_names;
  }
  if ( !boost::filesystem::is_directory( folder ) )
  {
    std::cerr << "\""<<dir_name<<"\" is not a folder.\n";
    return file_names;
  }
  boost::filesystem::directory_iterator end_iter;
  for (boost::filesystem::directory_iterator dir_itr(folder); dir_itr!=end_iter; ++dir_itr)
  {
    if (boost::filesystem::is_directory(*dir_itr))
      continue;
    std::string file_name = dir_itr->leaf();
    if (file_name.size()>=9 && file_name.substr(file_name.size()-9, 9)=="_info.txt")
    {
      file_names.push_back(file_name);
    }
  }
  std::sort(file_names.begin(), file_names.end());
  return file_names;
}

struct OpenniFrame
{
  OpenniFrame() : time_stamp_depth_image(0), frame_id_depth_image(0), depth_values(NULL),
                  width_depth_image(0), height_depth_image(0), focal_length_depth_image(0),
                  time_stamp_color_image(0), frame_id_color_image(0), color_values(NULL),
                  width_color_image(0), height_color_image(0), range_image_ptr(new RangeImagePlanar) {}
  ~OpenniFrame()
  {
    delete[] depth_values;
    delete[] color_values;
  }
  std::string depth_image_filename;
  unsigned long time_stamp_depth_image;
  int frame_id_depth_image;
  unsigned short* depth_values;
  int width_depth_image, height_depth_image;
  float focal_length_depth_image;
  std::string color_image_filename;
  unsigned long time_stamp_color_image;
  int frame_id_color_image;
  std::string color_image_encoding;
  unsigned char* color_values;
  int width_color_image, height_color_image;
  Eigen::Vector3f pose_diff_translation;
  Eigen::Quaternionf pose_diff_rotation;
  Eigen::Affine3f pose_diff;
  boost::shared_ptr<RangeImagePlanar> range_image_ptr;
};

void readOpenniFrame(istream& info_file, OpenniFrame& frame)
{
  while (!info_file.eof())
  {
    std::string parameter_name;
    info_file >> parameter_name;
    
    #define READ_INFO_FILE_PARAM(CURRENT_PARAM_NAME) \
      if (parameter_name == #CURRENT_PARAM_NAME) \
      { \
        info_file >> frame.CURRENT_PARAM_NAME; \
        if (0) \
          std::cout << PVARN(frame.CURRENT_PARAM_NAME); \
      }
    
    READ_INFO_FILE_PARAM(depth_image_filename)
    else READ_INFO_FILE_PARAM(time_stamp_depth_image)
    else READ_INFO_FILE_PARAM(frame_id_depth_image)
    else READ_INFO_FILE_PARAM(width_depth_image)
    else READ_INFO_FILE_PARAM(height_depth_image)
    else READ_INFO_FILE_PARAM(focal_length_depth_image)
    else READ_INFO_FILE_PARAM(color_image_filename)
    else READ_INFO_FILE_PARAM(time_stamp_color_image)
    else READ_INFO_FILE_PARAM(frame_id_color_image)
    else READ_INFO_FILE_PARAM(width_color_image)
    else READ_INFO_FILE_PARAM(height_color_image)
    else READ_INFO_FILE_PARAM(color_image_encoding)
    else if (parameter_name == "pose_diff_translation_xyz")
    {
      info_file >> frame.pose_diff_translation.x() >> frame.pose_diff_translation.y() >> frame.pose_diff_translation.z();
      //std::cout << "frame.pose_diff_translation = " << frame.pose_diff_translation.x()<<","
                                                    //<< frame.pose_diff_translation.x()<<","
                                                    //<< frame.pose_diff_translation.x()<<"\n";
    }
    else if (parameter_name == "pose_diff_rotation_wxyz")
    {
      info_file >> frame.pose_diff_rotation.w() >> frame.pose_diff_rotation.x()
                >> frame.pose_diff_rotation.y() >> frame.pose_diff_rotation.z();
      //std::cout << "frame.pose_diff_rotation = " << frame.pose_diff_rotation.w()<<","
                                                 //<< frame.pose_diff_rotation.x()<<","
                                                 //<< frame.pose_diff_rotation.y()<<","
                                                 //<< frame.pose_diff_rotation.z()<<"\n";
    }
    else
    {
      if (!parameter_name.empty())
        cout << "Unknown parameter name \""<<parameter_name<<"\"\n";
      std::string current_line;
      std::getline(info_file, current_line);
      continue;
    }
    
    #undef READ_INFO_FILE_PARAM
  }
  
  frame.pose_diff = Eigen::Translation3f(frame.pose_diff_translation) *
                    Eigen::Affine3f(frame.pose_diff_rotation);
  
  ifstream depth_image_file(frame.depth_image_filename.c_str());
  if (depth_image_file)
  {
    int depth_image_size = frame.width_depth_image*frame.height_depth_image*sizeof(*frame.depth_values);
    frame.depth_values = new unsigned short[depth_image_size];
    depth_image_file.read((char*)frame.depth_values, depth_image_size);
  }
  depth_image_file.close();
  
  ifstream color_image_file(frame.color_image_filename.c_str());
  if (color_image_file)
  {
    int color_image_size = frame.width_color_image*frame.height_color_image;
    if (frame.color_image_encoding=="RGB")
      color_image_size *= 3;
    else if (frame.color_image_encoding=="BAYER_GRBG")
      color_image_size *= 1;
    else if (frame.color_image_encoding=="YUV422")
      color_image_size *= 2;
    else
    {
      std::cerr << "Unknown color image encoding!\n";
    }
    frame.color_values = new unsigned char[color_image_size];
    color_image_file.read((char*)frame.color_values, color_image_size);
  }
  color_image_file.close();
}

bool save_point_clouds = false;

void
printUsage (const char* progName)
{
  cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
       << "Options:\n"
       << "-------------------------------------------\n"
       << "-r <float>      angular resolution in degrees (default "<<angular_resolution<<")\n"
       << "-t <int>        set the maximum number of threads for parallelization (default: "<<max_no_of_threads<<")\n"
       << "-m <float>      maximum range\n"
       << "-s              save point clouds to disc\n"
       << "-h              this help\n"
       << "\n\n";
}

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = deg2rad (angular_resolution);
  if (pcl::console::parse (argc, argv, "-m", maximum_range) >= 0)
    cout << "Setting maximum range to "<<maximum_range<<"m.\n";
  if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
    cout << "Setting maximum number of threads to "<< max_no_of_threads<<".\n";
  if (pcl::console::find_argument (argc, argv, "-s") >= 0)
  {
    save_point_clouds = true;
    cout << "Will save point clouds to disc.\n";
  }
  
  RangeImage::max_no_of_threads = max_no_of_threads;
  
  RangeImageVisualizer range_image_widget ("Range Image"),
                       color_image_widget ("RGB Image");
  
  PCLVisualizer viewer ("3D Viewer");
  viewer.addCoordinateSystem (1.0f);
  viewer.setBackgroundColor (1, 1, 1);
  
  viewer.initCameraParameters ();
  viewer.camera_.pos[0] = 0.0;
  viewer.camera_.pos[1] = -0.3;
  viewer.camera_.pos[2] = -2.0;
  viewer.camera_.focal[0] = 0.0;
  viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
  viewer.camera_.focal[2] = 1.0;
  viewer.camera_.view[0] = 0.0;
  viewer.camera_.view[1] = -1.0;
  viewer.camera_.view[2] = 0.0;
  viewer.updateCamera();
  
  std::vector<std::string> info_files = readInfoFiles(dir_name);
  if (info_files.empty())
  {
    std::cerr << "Did not find any info files.\n";
    return 1;
  }
  
  OpenniFrame* current_frame=NULL, * last_frame=NULL;
  Eigen::Affine3f current_pose = Eigen::Affine3f::Identity();
  
  VoxelGridCloud voxel_grid_cloud;
  
  for (int frame_idx=0; frame_idx<int(info_files.size()); ++frame_idx)
  {
    const std::string& info_file_name = info_files[frame_idx];
    std::ifstream info_file(info_file_name.c_str());
    if (!info_file)
    {
      std::cerr << "Could not open \""<<info_file_name<<"\".\n";
      return 1;
    }
    
    cout << "\nFrame "<<frame_idx+1<<":\n";
    delete last_frame;
    last_frame = current_frame;
    current_frame = new OpenniFrame();
    readOpenniFrame(info_file, *current_frame);
    info_file.close();
    
    OpenniFrame& frame = *current_frame;
    RangeImagePlanar& range_image = *frame.range_image_ptr;
    current_pose = current_pose * frame.pose_diff.inverse();
    
    range_image.setDepthImage (frame.depth_values,
        frame.width_depth_image, frame.height_depth_image,
        frame.width_depth_image/2, frame.height_depth_image/2,
        frame.focal_length_depth_image, frame.focal_length_depth_image,
        angular_resolution);
    
    if (maximum_range > 0.0f)
    {
      for (int point_idx=0; point_idx<range_image.points.size(); ++point_idx)
        if (range_image.points[point_idx].range > maximum_range)
          range_image.points[point_idx].range = std::numeric_limits<float>::infinity ();
    }
    
    if (show_color_image || save_point_clouds)
    {
      if (frame.color_image_encoding=="RGB")
        color_image_widget.setRGBImage(frame.color_values, frame.width_color_image, frame.height_color_image);
      else if(frame.color_image_encoding=="BAYER_GRBG")
        color_image_widget.setBayerGRBGImage(frame.color_values, frame.width_color_image, frame.height_color_image);
    }
    
    if (save_point_clouds)
    {
      int color_image_width, color_image_height;
      const unsigned char* colors = color_image_widget.getImageData(color_image_width, color_image_height);
      
      PointCloud<PointXYZRGB> colored_point_cloud;
      colored_point_cloud.sensor_origin_  = Eigen::Vector4f(current_pose(0,3), current_pose(1,3), current_pose(2,3), 0);
      colored_point_cloud.sensor_orientation_ = current_pose.rotation();
      for (int point_idx=0; point_idx<range_image.points.size(); ++point_idx)
      {
        if (range_image.isValid(point_idx))
        {
          PointXYZRGB colored_point;
          unsigned char& r = *(((unsigned char*)&colored_point.rgb)+2),
                       & g = *(((unsigned char*)&colored_point.rgb)+1),
                       & b = *(((unsigned char*)&colored_point.rgb)+0);
          r = g = b = 0;
          if (color_image_width==range_image.width && color_image_height==range_image.height)
            r=colors[3*point_idx], g=colors[3*point_idx+1], b=colors[3*point_idx+2];
          colored_point.getVector3fMap() = range_image.getPoint(point_idx).getVector3fMap();
          colored_point_cloud.points.push_back(colored_point);
        }
      }
      colored_point_cloud.width = colored_point_cloud.points.size();
      colored_point_cloud.height = 1;
      colored_point_cloud.is_dense = true;
      std::stringstream colored_point_cloud_name;
      //colored_point_cloud_name << setfill('0')<<setw(4)<<frame_idx+1<<"_colored_cloud.pcd";
      colored_point_cloud_name << "colored_cloud_" << setfill('0')<<setw(4)<<frame_idx+1<<".pcd";
      pcl::io::savePCDFile (colored_point_cloud_name.str(), colored_point_cloud, false);
    }
    
    // VISUALIZATION
    if (show_range_image)
      range_image_widget.setRangeImage (range_image, 0.0f, 10.0f);
    PointCloud<PointXYZ>::Ptr transformed_cloud_ptr(new PointCloud<PointXYZ>);
    
    if (show_registered_clouds)
    {
      PointCloud<PointXYZ>& transformed_cloud = *transformed_cloud_ptr;
      for (unsigned int i=0; i<range_image.points.size(); ++i)
      {
        if (range_image.isValid(i))
        {
          transformed_cloud.points.push_back(PointXYZ());
          transformed_cloud.points.back().getVector3fMap() = current_pose * range_image.points[i].getVector3fMap();
          voxel_grid_cloud.addPoint(transformed_cloud.points.back());
        }
      }
      
      PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler_transformed_cloud (transformed_cloud_ptr, 0, 0, 0);
      if (!viewer.updatePointCloud<pcl::PointXYZ> (transformed_cloud_ptr, color_handler_transformed_cloud,
                                                   "transformed cloud"))
        viewer.addPointCloud<pcl::PointXYZ> (transformed_cloud_ptr, color_handler_transformed_cloud,
                                             "transformed cloud");
    }
    
    for (int i=0; i<1; ++i)
    {
      if (show_registered_clouds)
        viewer.spinOnce (10);
      if (show_color_image || show_range_image)
        ImageWidgetWX::spinOnce ();  // process GUI events
      boost::this_thread::sleep (boost::posix_time::microseconds (1000));
    }
  }
  PointCloud<PointXYZ> complete_cloud;
  voxel_grid_cloud.getPointCloud(complete_cloud);
  cout << PVARN(complete_cloud.points.size());
  pcl::io::savePCDFile ("lala.pcd", complete_cloud, false);
  
}