00001
00002
00003 #include <iostream>
00004
00005 #include <boost/thread/thread.hpp>
00006 #include "pcl/range_image/range_image.h"
00007 #include "pcl/io/pcd_io.h"
00008
00009 #include "pcl/visualization/pcl_visualizer.h"
00010 #include "pcl/features/range_image_border_extractor.h"
00011 #include "pcl/keypoints/narf_keypoint.h"
00012 #include <pcl/console/parse.h>
00013
00014 #include <PointCloudUtils.hh>
00015
00016 typedef pcl::PointXYZ PointType;
00017
00018
00019
00020
00021 float angular_resolution = 0.5f;
00022 float support_size = 0.2f;
00023 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
00024 bool setUnseenToMaxRange = false;
00025
00026
00027
00028
00029 void
00030 printUsage (const char* progName)
00031 {
00032 std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
00033 << "Options:\n"
00034 << "-------------------------------------------\n"
00035 << "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
00036 << "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
00037 << "-m Treat all unseen points as maximum range readings\n"
00038 << "-s <float> support size for the interest points (diameter of the used sphere - "
00039 << "default "<<support_size<<")\n"
00040 << "-h this help\n"
00041 << "\n\n";
00042 }
00043
00044 void
00045 setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
00046 {
00047 Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
00048 Eigen::Vector3f look_at_vector = pcl::getRotationOnly (viewer_pose) * Eigen::Vector3f (0, 0, 1) + pos_vector;
00049 Eigen::Vector3f up_vector = pcl::getRotationOnly (viewer_pose) * Eigen::Vector3f (0, -1, 0);
00050 viewer.camera_.pos[0] = pos_vector[0];
00051 viewer.camera_.pos[1] = pos_vector[1];
00052 viewer.camera_.pos[2] = pos_vector[2];
00053 viewer.camera_.focal[0] = look_at_vector[0];
00054 viewer.camera_.focal[1] = look_at_vector[1];
00055 viewer.camera_.focal[2] = look_at_vector[2];
00056 viewer.camera_.view[0] = up_vector[0];
00057 viewer.camera_.view[1] = up_vector[1];
00058 viewer.camera_.view[2] = up_vector[2];
00059 viewer.updateCamera ();
00060 }
00061
00062
00063
00064
00065 int
00066 main (int argc, char** argv)
00067 {
00068
00069
00070
00071 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00072 {
00073 printUsage (argv[0]);
00074 return 0;
00075 }
00076 if (pcl::console::find_argument (argc, argv, "-m") >= 0)
00077 {
00078 setUnseenToMaxRange = true;
00079 cout << "Setting unseen values in range image to maximum range readings.\n";
00080 }
00081 int tmp_coordinate_frame;
00082 if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
00083 {
00084 coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
00085 cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
00086 }
00087 if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
00088 cout << "Setting support size to "<<support_size<<".\n";
00089 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00090 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00091 angular_resolution = pcl::deg2rad (angular_resolution);
00092
00093
00094
00095
00096 pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
00097 pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
00098 pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
00099 Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
00100 std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "wrl");
00101 if (!pcd_filename_indices.empty ())
00102 {
00103 std::string filename = argv[pcd_filename_indices[0]];
00104 point_cloud = lslgeneric::readVRML(filename.c_str());
00105
00106
00107
00108
00109
00110
00111
00112
00113 scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
00114 point_cloud.sensor_origin_[1],
00115 point_cloud.sensor_origin_[2])) *
00116 Eigen::Affine3f (point_cloud.sensor_orientation_);
00117 std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
00118 if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
00119 std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
00120 }
00121 else
00122 {
00123 cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
00124 for (float x=-0.5f; x<=0.5f; x+=0.01f)
00125 {
00126 for (float y=-0.5f; y<=0.5f; y+=0.01f)
00127 {
00128 PointType point;
00129 point.x = x;
00130 point.y = y;
00131 point.z = 2.0f - y;
00132 point_cloud.points.push_back (point);
00133 }
00134 }
00135 point_cloud.width = point_cloud.points.size ();
00136 point_cloud.height = 1;
00137 }
00138
00139
00140
00141
00142 float noise_level = 0.0;
00143 float min_range = 0.0f;
00144 int border_size = 1;
00145 boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
00146 pcl::RangeImage& range_image = *range_image_ptr;
00147 range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
00148 scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00149 range_image.integrateFarRanges (far_ranges);
00150 if (setUnseenToMaxRange)
00151 range_image.setUnseenToMaxRange ();
00152
00153
00154
00155
00156 pcl::visualization::PCLVisualizer viewer ("3D Viewer");
00157 viewer.setBackgroundColor (1, 1, 1);
00158 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
00159 viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
00160 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
00161
00162
00163
00164 viewer.initCameraParameters ();
00165 setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 pcl::RangeImageBorderExtractor range_image_border_extractor;
00177 pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
00178 narf_keypoint_detector.setRangeImage (&range_image);
00179 narf_keypoint_detector.getParameters ().support_size = support_size;
00180
00181
00182
00183 pcl::PointCloud<int> keypoint_indices;
00184 narf_keypoint_detector.compute (keypoint_indices);
00185 std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197 pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
00198 pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
00199 keypoints.points.resize (keypoint_indices.points.size ());
00200 for (size_t i=0; i<keypoint_indices.points.size (); ++i)
00201 keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
00202
00203 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
00204 viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
00205 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
00206
00207
00208
00209
00210 while (!viewer.wasStopped ())
00211 {
00212
00213 viewer.spinOnce (100);
00214 boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00215 }
00216 }
00217