00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <set>
00011 #include <map>
00012 #include <iostream>
00013 using namespace std;
00014 #include "pcl/range_image/range_image_planar.h"
00015 #include "pcl/common/common_headers.h"
00016 #include "pcl/visualization/range_image_visualizer.h"
00017 #include "pcl/visualization/pcl_visualizer.h"
00018 #include "pcl/console/parse.h"
00019 #include <boost/filesystem.hpp>
00020 #include "pcl/io/pcd_io.h"
00021 #include <boost/thread/thread.hpp>
00022
00023
00024
00025 using namespace pcl;
00026 using namespace pcl::visualization;
00027
00028 std::string dir_name = ".";
00029
00030 float angular_resolution = -1;
00031
00032 int icp_search_radius_consecutive = 3;
00033 float icp_max_distance_start_consecutive=0.3, icp_max_distance_end_consecutive=0.05;
00034 int icp_num_iterations_consecutive = 20;
00035 int icp_search_radius_keyframe = 1;
00036 float icp_max_distance_start_keyframe=0.15, icp_max_distance_end_keyframe=0.05;
00037 int icp_num_iterations_keyframe = 10;
00038 float overlap_calculation_max_error = 0.25;
00039 float new_keyframe_overlap = 0.85;
00040 int max_no_of_threads = 1;
00041 float output_resolution = 0.005;
00042
00043 float maximum_range = -1.0f;
00044
00045 struct VoxelGridCloud {
00046 VoxelGridCloud(float resolution=0.01) : resolution(resolution) {}
00047 template <typename PointType>
00048 void addPoint(const PointType& point)
00049 {
00050 unsigned int cell_x = lrintf(point.x/resolution) + (std::numeric_limits<unsigned int>::max()/2),
00051 cell_y = lrintf(point.y/resolution) + (std::numeric_limits<unsigned int>::max()/2),
00052 cell_z = lrintf(point.z/resolution) + (std::numeric_limits<unsigned int>::max()/2);
00053 voxels[cell_x][cell_y].insert(cell_z);
00054 }
00055 void getPointCloud(PointCloud<PointXYZ>& cloud) const
00056 {
00057 for (std::map<unsigned int, std::map<unsigned int, std::set<unsigned int> > >::const_iterator it_x=voxels.begin();
00058 it_x!=voxels.end(); ++it_x)
00059 {
00060 float x = (double(it_x->first)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
00061 for (std::map<unsigned int, std::set<unsigned int> >::const_iterator it_y=it_x->second.begin();
00062 it_y!=it_x->second.end(); ++it_y)
00063 {
00064 float y = (double(it_y->first)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
00065 for (std::set<unsigned int>::const_iterator it_z=it_y->second.begin();
00066 it_z!=it_y->second.end(); ++it_z)
00067 {
00068 float z = (double(*it_z)-double(std::numeric_limits<unsigned int>::max()/2))*resolution;
00069 cloud.points.push_back(PointXYZ(x, y, z));
00070 }
00071 }
00072 }
00073 }
00074 std::map<unsigned int, std::map<unsigned int, std::set<unsigned int> > > voxels;
00075 float resolution;
00076 };
00077
00078 bool show_color_image = 1,
00079 show_range_image = 1,
00080 show_registered_clouds = 1;
00081
00082 Eigen::Affine3f last_pose, current_pose=Eigen::Affine3f::Identity();
00083
00084 std::vector<std::string> readInfoFiles(const std::string& folder_name)
00085 {
00086 std::vector<std::string> file_names;
00087 boost::filesystem::path folder( folder_name );
00088 if ( !boost::filesystem::exists( folder ) )
00089 {
00090 std::cerr << "Folder \""<<dir_name<<"\" does not exist.\n";
00091 return file_names;
00092 }
00093 if ( !boost::filesystem::is_directory( folder ) )
00094 {
00095 std::cerr << "\""<<dir_name<<"\" is not a folder.\n";
00096 return file_names;
00097 }
00098 boost::filesystem::directory_iterator end_iter;
00099 for (boost::filesystem::directory_iterator dir_itr(folder); dir_itr!=end_iter; ++dir_itr)
00100 {
00101 if (boost::filesystem::is_directory(*dir_itr))
00102 continue;
00103 std::string file_name = dir_itr->leaf();
00104 if (file_name.size()>=9 && file_name.substr(file_name.size()-9, 9)=="_info.txt")
00105 {
00106 file_names.push_back(file_name);
00107 }
00108 }
00109 std::sort(file_names.begin(), file_names.end());
00110 return file_names;
00111 }
00112
00113 struct OpenniFrame
00114 {
00115 OpenniFrame() : time_stamp_depth_image(0), frame_id_depth_image(0), depth_values(NULL),
00116 width_depth_image(0), height_depth_image(0), focal_length_depth_image(0),
00117 time_stamp_color_image(0), frame_id_color_image(0), color_values(NULL),
00118 width_color_image(0), height_color_image(0), range_image_ptr(new RangeImagePlanar) {}
00119 ~OpenniFrame()
00120 {
00121 delete[] depth_values;
00122 delete[] color_values;
00123 }
00124 std::string depth_image_filename;
00125 unsigned long time_stamp_depth_image;
00126 int frame_id_depth_image;
00127 unsigned short* depth_values;
00128 int width_depth_image, height_depth_image;
00129 float focal_length_depth_image;
00130 std::string color_image_filename;
00131 unsigned long time_stamp_color_image;
00132 int frame_id_color_image;
00133 std::string color_image_encoding;
00134 unsigned char* color_values;
00135 int width_color_image, height_color_image;
00136 Eigen::Vector3f pose_diff_translation;
00137 Eigen::Quaternionf pose_diff_rotation;
00138 Eigen::Affine3f pose_diff;
00139 boost::shared_ptr<RangeImagePlanar> range_image_ptr;
00140 };
00141
00142 void readOpenniFrame(istream& info_file, OpenniFrame& frame)
00143 {
00144 while (!info_file.eof())
00145 {
00146 std::string parameter_name;
00147 info_file >> parameter_name;
00148
00149 #define READ_INFO_FILE_PARAM(CURRENT_PARAM_NAME) \
00150 if (parameter_name == #CURRENT_PARAM_NAME) \
00151 { \
00152 info_file >> frame.CURRENT_PARAM_NAME; \
00153 if (0) \
00154 std::cout << PVARN(frame.CURRENT_PARAM_NAME); \
00155 }
00156
00157 READ_INFO_FILE_PARAM(depth_image_filename)
00158 else READ_INFO_FILE_PARAM(time_stamp_depth_image)
00159 else READ_INFO_FILE_PARAM(frame_id_depth_image)
00160 else READ_INFO_FILE_PARAM(width_depth_image)
00161 else READ_INFO_FILE_PARAM(height_depth_image)
00162 else READ_INFO_FILE_PARAM(focal_length_depth_image)
00163 else READ_INFO_FILE_PARAM(color_image_filename)
00164 else READ_INFO_FILE_PARAM(time_stamp_color_image)
00165 else READ_INFO_FILE_PARAM(frame_id_color_image)
00166 else READ_INFO_FILE_PARAM(width_color_image)
00167 else READ_INFO_FILE_PARAM(height_color_image)
00168 else READ_INFO_FILE_PARAM(color_image_encoding)
00169 else if (parameter_name == "pose_diff_translation_xyz")
00170 {
00171 info_file >> frame.pose_diff_translation.x() >> frame.pose_diff_translation.y() >> frame.pose_diff_translation.z();
00172
00173
00174
00175 }
00176 else if (parameter_name == "pose_diff_rotation_wxyz")
00177 {
00178 info_file >> frame.pose_diff_rotation.w() >> frame.pose_diff_rotation.x()
00179 >> frame.pose_diff_rotation.y() >> frame.pose_diff_rotation.z();
00180
00181
00182
00183
00184 }
00185 else
00186 {
00187 if (!parameter_name.empty())
00188 cout << "Unknown parameter name \""<<parameter_name<<"\"\n";
00189 std::string current_line;
00190 std::getline(info_file, current_line);
00191 continue;
00192 }
00193
00194 #undef READ_INFO_FILE_PARAM
00195 }
00196
00197 frame.pose_diff = Eigen::Translation3f(frame.pose_diff_translation) *
00198 Eigen::Affine3f(frame.pose_diff_rotation);
00199
00200 ifstream depth_image_file(frame.depth_image_filename.c_str());
00201 if (depth_image_file)
00202 {
00203 int depth_image_size = frame.width_depth_image*frame.height_depth_image*sizeof(*frame.depth_values);
00204 frame.depth_values = new unsigned short[depth_image_size];
00205 depth_image_file.read((char*)frame.depth_values, depth_image_size);
00206 }
00207 depth_image_file.close();
00208
00209 ifstream color_image_file(frame.color_image_filename.c_str());
00210 if (color_image_file)
00211 {
00212 int color_image_size = frame.width_color_image*frame.height_color_image;
00213 if (frame.color_image_encoding=="RGB")
00214 color_image_size *= 3;
00215 else if (frame.color_image_encoding=="BAYER_GRBG")
00216 color_image_size *= 1;
00217 else if (frame.color_image_encoding=="YUV422")
00218 color_image_size *= 2;
00219 else
00220 {
00221 std::cerr << "Unknown color image encoding!\n";
00222 }
00223 frame.color_values = new unsigned char[color_image_size];
00224 color_image_file.read((char*)frame.color_values, color_image_size);
00225 }
00226 color_image_file.close();
00227 }
00228
00229 bool save_point_clouds = false;
00230
00231 void
00232 printUsage (const char* progName)
00233 {
00234 cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
00235 << "Options:\n"
00236 << "-------------------------------------------\n"
00237 << "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
00238 << "-t <int> set the maximum number of threads for parallelization (default: "<<max_no_of_threads<<")\n"
00239 << "-m <float> maximum range\n"
00240 << "-s save point clouds to disc\n"
00241 << "-h this help\n"
00242 << "\n\n";
00243 }
00244
00245 int main (int argc, char** argv)
00246 {
00247
00248
00249
00250 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00251 {
00252 printUsage (argv[0]);
00253 return 0;
00254 }
00255 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00256 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00257 angular_resolution = deg2rad (angular_resolution);
00258 if (pcl::console::parse (argc, argv, "-m", maximum_range) >= 0)
00259 cout << "Setting maximum range to "<<maximum_range<<"m.\n";
00260 if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
00261 cout << "Setting maximum number of threads to "<< max_no_of_threads<<".\n";
00262 if (pcl::console::find_argument (argc, argv, "-s") >= 0)
00263 {
00264 save_point_clouds = true;
00265 cout << "Will save point clouds to disc.\n";
00266 }
00267
00268 RangeImage::max_no_of_threads = max_no_of_threads;
00269
00270 RangeImageVisualizer range_image_widget ("Range Image"),
00271 color_image_widget ("RGB Image");
00272
00273 PCLVisualizer viewer ("3D Viewer");
00274 viewer.addCoordinateSystem (1.0f);
00275 viewer.setBackgroundColor (1, 1, 1);
00276
00277 viewer.initCameraParameters ();
00278 viewer.camera_.pos[0] = 0.0;
00279 viewer.camera_.pos[1] = -0.3;
00280 viewer.camera_.pos[2] = -2.0;
00281 viewer.camera_.focal[0] = 0.0;
00282 viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
00283 viewer.camera_.focal[2] = 1.0;
00284 viewer.camera_.view[0] = 0.0;
00285 viewer.camera_.view[1] = -1.0;
00286 viewer.camera_.view[2] = 0.0;
00287 viewer.updateCamera();
00288
00289 std::vector<std::string> info_files = readInfoFiles(dir_name);
00290 if (info_files.empty())
00291 {
00292 std::cerr << "Did not find any info files.\n";
00293 return 1;
00294 }
00295
00296 OpenniFrame* current_frame=NULL, * last_frame=NULL;
00297 Eigen::Affine3f current_pose = Eigen::Affine3f::Identity();
00298
00299 VoxelGridCloud voxel_grid_cloud;
00300
00301 for (int frame_idx=0; frame_idx<int(info_files.size()); ++frame_idx)
00302 {
00303 const std::string& info_file_name = info_files[frame_idx];
00304 std::ifstream info_file(info_file_name.c_str());
00305 if (!info_file)
00306 {
00307 std::cerr << "Could not open \""<<info_file_name<<"\".\n";
00308 return 1;
00309 }
00310
00311 cout << "\nFrame "<<frame_idx+1<<":\n";
00312 delete last_frame;
00313 last_frame = current_frame;
00314 current_frame = new OpenniFrame();
00315 readOpenniFrame(info_file, *current_frame);
00316 info_file.close();
00317
00318 OpenniFrame& frame = *current_frame;
00319 RangeImagePlanar& range_image = *frame.range_image_ptr;
00320 current_pose = current_pose * frame.pose_diff.inverse();
00321
00322 range_image.setDepthImage (frame.depth_values,
00323 frame.width_depth_image, frame.height_depth_image,
00324 frame.width_depth_image/2, frame.height_depth_image/2,
00325 frame.focal_length_depth_image, frame.focal_length_depth_image,
00326 angular_resolution);
00327
00328 if (maximum_range > 0.0f)
00329 {
00330 for (int point_idx=0; point_idx<range_image.points.size(); ++point_idx)
00331 if (range_image.points[point_idx].range > maximum_range)
00332 range_image.points[point_idx].range = std::numeric_limits<float>::infinity ();
00333 }
00334
00335 if (show_color_image || save_point_clouds)
00336 {
00337 if (frame.color_image_encoding=="RGB")
00338 color_image_widget.setRGBImage(frame.color_values, frame.width_color_image, frame.height_color_image);
00339 else if(frame.color_image_encoding=="BAYER_GRBG")
00340 color_image_widget.setBayerGRBGImage(frame.color_values, frame.width_color_image, frame.height_color_image);
00341 }
00342
00343 if (save_point_clouds)
00344 {
00345 int color_image_width, color_image_height;
00346 const unsigned char* colors = color_image_widget.getImageData(color_image_width, color_image_height);
00347
00348 PointCloud<PointXYZRGB> colored_point_cloud;
00349 colored_point_cloud.sensor_origin_ = Eigen::Vector4f(current_pose(0,3), current_pose(1,3), current_pose(2,3), 0);
00350 colored_point_cloud.sensor_orientation_ = current_pose.rotation();
00351 for (int point_idx=0; point_idx<range_image.points.size(); ++point_idx)
00352 {
00353 if (range_image.isValid(point_idx))
00354 {
00355 PointXYZRGB colored_point;
00356 unsigned char& r = *(((unsigned char*)&colored_point.rgb)+2),
00357 & g = *(((unsigned char*)&colored_point.rgb)+1),
00358 & b = *(((unsigned char*)&colored_point.rgb)+0);
00359 r = g = b = 0;
00360 if (color_image_width==range_image.width && color_image_height==range_image.height)
00361 r=colors[3*point_idx], g=colors[3*point_idx+1], b=colors[3*point_idx+2];
00362 colored_point.getVector3fMap() = range_image.getPoint(point_idx).getVector3fMap();
00363 colored_point_cloud.points.push_back(colored_point);
00364 }
00365 }
00366 colored_point_cloud.width = colored_point_cloud.points.size();
00367 colored_point_cloud.height = 1;
00368 colored_point_cloud.is_dense = true;
00369 std::stringstream colored_point_cloud_name;
00370
00371 colored_point_cloud_name << "colored_cloud_" << setfill('0')<<setw(4)<<frame_idx+1<<".pcd";
00372 pcl::io::savePCDFile (colored_point_cloud_name.str(), colored_point_cloud, false);
00373 }
00374
00375
00376 if (show_range_image)
00377 range_image_widget.setRangeImage (range_image, 0.0f, 10.0f);
00378 PointCloud<PointXYZ>::Ptr transformed_cloud_ptr(new PointCloud<PointXYZ>);
00379
00380 if (show_registered_clouds)
00381 {
00382 PointCloud<PointXYZ>& transformed_cloud = *transformed_cloud_ptr;
00383 for (unsigned int i=0; i<range_image.points.size(); ++i)
00384 {
00385 if (range_image.isValid(i))
00386 {
00387 transformed_cloud.points.push_back(PointXYZ());
00388 transformed_cloud.points.back().getVector3fMap() = current_pose * range_image.points[i].getVector3fMap();
00389 voxel_grid_cloud.addPoint(transformed_cloud.points.back());
00390 }
00391 }
00392
00393 PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler_transformed_cloud (transformed_cloud_ptr, 0, 0, 0);
00394 if (!viewer.updatePointCloud<pcl::PointXYZ> (transformed_cloud_ptr, color_handler_transformed_cloud,
00395 "transformed cloud"))
00396 viewer.addPointCloud<pcl::PointXYZ> (transformed_cloud_ptr, color_handler_transformed_cloud,
00397 "transformed cloud");
00398 }
00399
00400 for (int i=0; i<1; ++i)
00401 {
00402 if (show_registered_clouds)
00403 viewer.spinOnce (10);
00404 if (show_color_image || show_range_image)
00405 ImageWidgetWX::spinOnce ();
00406 boost::this_thread::sleep (boost::posix_time::microseconds (1000));
00407 }
00408 }
00409 PointCloud<PointXYZ> complete_cloud;
00410 voxel_grid_cloud.getPointCloud(complete_cloud);
00411 cout << PVARN(complete_cloud.points.size());
00412 pcl::io::savePCDFile ("lala.pcd", complete_cloud, false);
00413
00414 }