00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <iostream>
00042 using namespace std;
00043
00044 #include <boost/thread/thread.hpp>
00045 #include "pcl/common/common_headers.h"
00046 #include "pcl/common/common_headers.h"
00047 #include "pcl/range_image/range_image.h"
00048 #include "pcl/io/pcd_io.h"
00049 #include "pcl/visualization/range_image_visualizer.h"
00050 #include "pcl/visualization/pcl_visualizer.h"
00051 #include <pcl/console/parse.h>
00052
00053 using namespace pcl;
00054 using namespace pcl::visualization;
00055 typedef PointXYZ PointType;
00056
00057
00058
00059
00060 float angular_resolution = 0.5f;
00061 RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
00062 bool live_update = false;
00063
00064
00065
00066
00067 void printUsage (const char* progName)
00068 {
00069 cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
00070 << "Options:\n"
00071 << "-------------------------------------------\n"
00072 << "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
00073 << "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
00074 << "-l live update - update the range image according to the selected view in the 3D viewer.\n"
00075 << "-h this help\n"
00076 << "\n\n";
00077 }
00078
00079 void setViewerPose (PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
00080 {
00081 Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
00082 Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
00083 Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
00084 viewer.camera_.pos[0] = pos_vector[0];
00085 viewer.camera_.pos[1] = pos_vector[1];
00086 viewer.camera_.pos[2] = pos_vector[2];
00087 viewer.camera_.focal[0] = look_at_vector[0];
00088 viewer.camera_.focal[1] = look_at_vector[1];
00089 viewer.camera_.focal[2] = look_at_vector[2];
00090 viewer.camera_.view[0] = up_vector[0];
00091 viewer.camera_.view[1] = up_vector[1];
00092 viewer.camera_.view[2] = up_vector[2];
00093 viewer.updateCamera();
00094 }
00095
00096
00097
00098
00099 int main (int argc, char** argv)
00100 {
00101
00102
00103
00104 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00105 {
00106 printUsage (argv[0]);
00107 return 0;
00108 }
00109 if (pcl::console::find_argument (argc, argv, "-l") >= 0)
00110 {
00111 live_update = true;
00112 cout << "Live update is on.\n";
00113 }
00114 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00115 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00116 int tmp_coordinate_frame;
00117 if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
00118 {
00119 coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
00120 cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
00121 }
00122 angular_resolution = deg2rad (angular_resolution);
00123
00124
00125
00126
00127 pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
00128 pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
00129 Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
00130 std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
00131 if (!pcd_filename_indices.empty ())
00132 {
00133 std::string filename = argv[pcd_filename_indices[0]];
00134 if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
00135 {
00136 cerr << "Was not able to open file \""<<filename<<"\".\n";
00137 printUsage (argv[0]);
00138 return 0;
00139 }
00140 scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
00141 point_cloud.sensor_origin_[1],
00142 point_cloud.sensor_origin_[2])) *
00143 Eigen::Affine3f (point_cloud.sensor_orientation_);
00144 }
00145 else
00146 {
00147 cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
00148 for (float x=-0.5f; x<=0.5f; x+=0.01f)
00149 {
00150 for (float y=-0.5f; y<=0.5f; y+=0.01f)
00151 {
00152 PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
00153 point_cloud.points.push_back (point);
00154 }
00155 }
00156 point_cloud.width = point_cloud.points.size (); point_cloud.height = 1;
00157 }
00158
00159
00160
00161
00162 float noise_level = 0.0;
00163 float min_range = 0.0f;
00164 int border_size = 1;
00165 boost::shared_ptr<RangeImage> range_image_ptr(new RangeImage);
00166 RangeImage& range_image = *range_image_ptr;
00167 range_image.createFromPointCloud (point_cloud, angular_resolution, deg2rad (360.0f), deg2rad (180.0f),
00168 scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
00169
00170
00171
00172
00173 PCLVisualizer viewer ("3D Viewer");
00174 viewer.setBackgroundColor (1, 1, 1);
00175 PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
00176
00177
00178
00179 PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
00180 viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
00181 viewer.initCameraParameters ();
00182 setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
00183
00184
00185
00186
00187 RangeImageVisualizer range_image_widget ("Range image");
00188 range_image_widget.showRangeImage (range_image);
00189
00190
00191
00192
00193 std::vector<pcl::visualization::Camera> cameras;
00194 while (!viewer.wasStopped ())
00195 {
00196 range_image_widget.spinOnce ();
00197 viewer.spinOnce (100);
00198 pcl_sleep (1);
00199
00200 if (live_update)
00201 {
00202 scene_sensor_pose = viewer.getViewerPose();
00203 range_image.createFromPointCloud (point_cloud, angular_resolution, deg2rad (360.0f), deg2rad (180.0f),
00204 scene_sensor_pose, RangeImage::LASER_FRAME, noise_level, min_range, border_size);
00205 range_image_widget.showRangeImage (range_image);
00206 }
00207 }
00208 }