Go to the documentation of this file.00001
00002
00003 #include <iostream>
00004 using namespace std;
00005
00006 #include "pcl/range_image/range_image.h"
00007 #include "pcl/io/pcd_io.h"
00008 #include "pcl/visualization/range_image_visualizer.h"
00009 #include "pcl/visualization/pcl_visualizer.h"
00010 #include "pcl/features/range_image_border_extractor.h"
00011 #include "pcl/keypoints/narf_keypoint.h"
00012
00013 using namespace pcl;
00014 using namespace pcl::visualization;
00015 typedef PointXYZ PointType;
00016
00017
00018
00019
00020 float angular_resolution = 0.5f;
00021 float support_size = 0.2f;
00022 RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
00023 bool setUnseenToMaxRange = false;
00024
00025
00026
00027
00028 void printUsage(const char* progName)
00029 {
00030 cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
00031 << "Options:\n"
00032 << "-------------------------------------------\n"
00033 << "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
00034 << "-c <int> coordinate frame (default "<<(int)coordinate_frame<<")\n"
00035 << "-m Treat all unseen points to max range\n"
00036 << "-s <float> support size for the interest points (diameter of the used sphere in meters) (default "<<support_size<<")\n"
00037 << "-h this help\n"
00038 << "\n\n";
00039 }
00040
00041
00042
00043
00044 int main (int argc, char** argv)
00045 {
00046
00047
00048
00049 for (char c; (c = getopt(argc, argv, "r:c:ms:h")) != -1; ) {
00050 switch (c) {
00051 case 'r':
00052 {
00053 angular_resolution = strtod(optarg, NULL);
00054 cout << "Setting angular resolution to "<<angular_resolution<<".\n";
00055 break;
00056 }
00057 case 'c':
00058 {
00059 coordinate_frame = (RangeImage::CoordinateFrame)strtol(optarg, NULL, 0);
00060 cout << "Using coordinate frame "<<(int)coordinate_frame<<".\n";
00061 break;
00062 }
00063 case 'm':
00064 {
00065 setUnseenToMaxRange = true;
00066 break;
00067 }
00068 case 's':
00069 {
00070 support_size = strtod(optarg, NULL);
00071 cout << "Setting support size to "<<support_size<<".\n";
00072 break;
00073 }
00074 case 'h':
00075 printUsage(argv[0]);
00076 exit(0);
00077 }
00078 }
00079 angular_resolution = deg2rad(angular_resolution);
00080
00081
00082
00083
00084
00085 pcl::PointCloud<PointType> point_cloud;
00086 PointCloud<PointWithViewpoint> far_ranges;
00087 Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
00088 if (optind < argc)
00089 {
00090 sensor_msgs::PointCloud2 point_cloud_data;
00091 if (pcl::io::loadPCDFile(argv[optind], point_cloud_data) == -1)
00092 {
00093 cerr << "Was not able to open file \""<<argv[optind]<<"\".\n";
00094 printUsage(argv[0]);
00095 exit(0);
00096 }
00097 fromROSMsg(point_cloud_data, point_cloud);
00098 RangeImage::extractFarRanges(point_cloud_data, far_ranges);
00099 if (pcl::getFieldIndex(point_cloud_data, "vp_x")>=0)
00100 {
00101 cout << "Scene point cloud has viewpoint information.\n";
00102 PointCloud<PointWithViewpoint> tmp_pc; fromROSMsg(point_cloud_data, tmp_pc);
00103 Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
00104 scene_sensor_pose = Eigen::Translation3f(average_viewpoint) * scene_sensor_pose;
00105 }
00106 }
00107 else
00108 {
00109 cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
00110 for (float x=-0.5f; x<=0.5f; x+=0.01f)
00111 {
00112 for (float y=-0.5f; y<=0.5f; y+=0.01f)
00113 {
00114 PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
00115 point_cloud.points.push_back(point);
00116 }
00117 }
00118 point_cloud.width = point_cloud.points.size(); point_cloud.height = 1;
00119 }
00120
00121
00122
00123
00124
00125 float noise_level = 0.0;
00126 float min_range = 0.0f;
00127 int border_size = 1;
00128 RangeImage range_image;
00129 range_image.createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f),
00130 scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00131 range_image.integrateFarRanges(far_ranges);
00132 if (setUnseenToMaxRange)
00133 range_image.setUnseenToMaxRange();
00134
00135
00136
00137
00138
00139 RangeImageBorderExtractor range_image_border_extractor;
00140 NarfKeypoint narf_keypoint_detector;
00141 narf_keypoint_detector.setRangeImageBorderExtractor(&range_image_border_extractor);
00142 narf_keypoint_detector.setRangeImage(&range_image);
00143 narf_keypoint_detector.getParameters().support_size = support_size;
00144
00145
00146
00147 PointCloud<int> keypoint_indices;
00148 narf_keypoint_detector.compute(keypoint_indices);
00149 std::cout << "Found "<<keypoint_indices.points.size()<<" key points.\n";
00150
00151
00152
00153
00154 PCLVisualizer viewer("3D Viewer");
00155 viewer.addCoordinateSystem(1.0f);
00156 viewer.addPointCloud(point_cloud.makeShared(), "original point cloud");
00157
00158
00159
00160
00161
00162 RangeImageVisualizer range_image_widget("Range image");
00163 range_image_widget.showRangeImage(range_image);
00164
00165
00166
00167
00168 for (size_t i=0; i<keypoint_indices.points.size(); ++i)
00169 range_image_widget.markPoint(keypoint_indices.points[i]%range_image.width,
00170 keypoint_indices.points[i]/range_image.width,
00171 pcl::visualization::Vector3ub (0,255,0));
00172
00173
00174
00175
00176 PointCloud<PointXYZ> keypoints;
00177 keypoints.points.resize(keypoint_indices.points.size());
00178 for (size_t i=0; i<keypoint_indices.points.size(); ++i)
00179 keypoints.points[i].getVector3fMap() = range_image.points[keypoint_indices.points[i]].getVector3fMap();
00180 viewer.addPointCloud(keypoints.makeShared(), "keypoints");
00181 viewer.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
00182
00183
00184
00185
00186 while(!viewer.wasStopped())
00187 {
00188 range_image_widget.spinOnce();
00189 viewer.spinOnce(100);
00190 usleep(100000);
00191 }
00192 }