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/console/parse.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 RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
00022 bool setUnseenToMaxRange = false;
00023
00024
00025
00026
00027 void printUsage (const char* progName)
00028 {
00029 cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
00030 << "Options:\n"
00031 << "-------------------------------------------\n"
00032 << "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
00033 << "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
00034 << "-m Treat all unseen points to max range\n"
00035 << "-h this help\n"
00036 << "\n\n";
00037 }
00038
00039
00040
00041
00042 int main (int argc, char** argv)
00043 {
00044
00045
00046
00047 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00048 {
00049 printUsage (argv[0]);
00050 return 0;
00051 }
00052 if (pcl::console::find_argument (argc, argv, "-m") >= 0)
00053 {
00054 setUnseenToMaxRange = true;
00055 cout << "Setting unseen values in range image to maximum range readings.\n";
00056 }
00057 int tmp_coordinate_frame;
00058 if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
00059 {
00060 coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
00061 cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
00062 }
00063 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00064 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00065 angular_resolution = deg2rad (angular_resolution);
00066
00067
00068
00069
00070 pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
00071 pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
00072 PointCloud<PointWithViewpoint> far_ranges;
00073 Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
00074 std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
00075 if (!pcd_filename_indices.empty ())
00076 {
00077 std::string filename = argv[pcd_filename_indices[0]];
00078 if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
00079 {
00080 cerr << "Was not able to open file \""<<filename<<"\".\n";
00081 printUsage (argv[0]);
00082 return 0;
00083 }
00084 scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
00085 point_cloud.sensor_origin_[1],
00086 point_cloud.sensor_origin_[2])) *
00087 Eigen::Affine3f (point_cloud.sensor_orientation_);
00088 }
00089 else
00090 {
00091 cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
00092 for (float x=-0.5f; x<=0.5f; x+=0.01f)
00093 {
00094 for (float y=-0.5f; y<=0.5f; y+=0.01f)
00095 {
00096 PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
00097 point_cloud.points.push_back (point);
00098 }
00099 }
00100 point_cloud.width = point_cloud.points.size (); point_cloud.height = 1;
00101 }
00102
00103
00104
00105
00106 float noise_level = 0.0;
00107 float min_range = 0.0f;
00108 int border_size = 1;
00109 boost::shared_ptr<RangeImage> range_image_ptr(new RangeImage);
00110 RangeImage& range_image = *range_image_ptr;
00111 range_image.createFromPointCloud (point_cloud, angular_resolution, deg2rad (360.0f), deg2rad (180.0f),
00112 scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00113 range_image.integrateFarRanges (far_ranges);
00114 if (setUnseenToMaxRange)
00115 range_image.setUnseenToMaxRange ();
00116
00117
00118
00119
00120 PCLVisualizer viewer ("3D Viewer");
00121 viewer.setBackgroundColor (1, 1, 1);
00122 viewer.addCoordinateSystem (1.0f);
00123 PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
00124 viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
00125
00126
00127
00128
00129
00130
00131
00132 RangeImageBorderExtractor border_extractor (&range_image);
00133 PointCloud<BorderDescription> border_descriptions;
00134 border_extractor.compute (border_descriptions);
00135
00136
00137
00138
00139 pcl::PointCloud<PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<PointWithRange>),
00140 veil_points_ptr(new pcl::PointCloud<PointWithRange>),
00141 shadow_points_ptr(new pcl::PointCloud<PointWithRange>);
00142 pcl::PointCloud<PointWithRange>& border_points = *border_points_ptr,
00143 & veil_points = * veil_points_ptr,
00144 & shadow_points = *shadow_points_ptr;
00145 for (int y=0; y< (int)range_image.height; ++y)
00146 {
00147 for (int x=0; x< (int)range_image.width; ++x)
00148 {
00149 if (border_descriptions.points[y*range_image.width + x].traits[BORDER_TRAIT__OBSTACLE_BORDER])
00150 border_points.points.push_back (range_image.points[y*range_image.width + x]);
00151 if (border_descriptions.points[y*range_image.width + x].traits[BORDER_TRAIT__VEIL_POINT])
00152 veil_points.points.push_back (range_image.points[y*range_image.width + x]);
00153 if (border_descriptions.points[y*range_image.width + x].traits[BORDER_TRAIT__SHADOW_BORDER])
00154 shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
00155 }
00156 }
00157 PointCloudColorHandlerCustom<PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
00158 viewer.addPointCloud<PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
00159 viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 7, "border points");
00160 PointCloudColorHandlerCustom<PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
00161 viewer.addPointCloud<PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
00162 viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
00163 PointCloudColorHandlerCustom<PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
00164 viewer.addPointCloud<PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
00165 viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");
00166
00167
00168
00169
00170 RangeImageVisualizer* range_image_borders_widget = NULL;
00171 range_image_borders_widget =
00172 RangeImageVisualizer::getRangeImageBordersWidget (range_image, -INFINITY, INFINITY, false,
00173 border_descriptions, "Range image with borders");
00174
00175
00176
00177
00178
00179
00180 while (!viewer.wasStopped () && range_image_borders_widget->isShown ())
00181 {
00182 ImageWidgetWX::spinOnce ();
00183 viewer.spinOnce (100);
00184 usleep (100000);
00185 }
00186 }