Go to the documentation of this file.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 #include <sensor_msgs/PointCloud2.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/console/print.h>
00043 #include <pcl/console/parse.h>
00044 #include <pcl/console/time.h>
00045 #include <pcl/kdtree/kdtree_flann.h>
00046
00047
00048 using namespace pcl;
00049 using namespace pcl::io;
00050 using namespace pcl::console;
00051
00052 std::string default_correspondence_type = "index";
00053
00054 void
00055 printHelp (int, char **argv)
00056 {
00057 print_error ("Syntax is: %s source.pcd target.pcd output_intensity.pcd <options>\n", argv[0]);
00058 print_info (" where options are:\n");
00059 print_info (" -correspondence X = the way of selecting the corresponding pair in the target cloud for the current point in the source cloud\n");
00060 print_info (" options are: index = points with identical indices are paired together. Note: both clouds need to have the same number of points\n");
00061 print_info (" nn = source point is paired with its nearest neighbor in the target cloud\n");
00062 print_info (" nnplane = source point is paired with its projection on the plane determined by the nearest neighbor in the target cloud. Note: target cloud needs to contain normals\n");
00063 print_info (" (default: ");
00064 print_value ("%s", default_correspondence_type.c_str ()); print_info (")\n");
00065 }
00066
00067 bool
00068 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
00069 {
00070 TicToc tt;
00071
00072
00073 tt.tic ();
00074 if (loadPCDFile (filename, cloud) < 0)
00075 return (false);
00076
00077
00078
00079 return (true);
00080 }
00081
00082 void
00083 compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_msgs::PointCloud2::ConstPtr &cloud_target,
00084 sensor_msgs::PointCloud2 &output, std::string correspondence_type)
00085 {
00086
00087 TicToc tt;
00088 tt.tic ();
00089
00090 PointCloud<PointXYZ>::Ptr xyz_source (new PointCloud<PointXYZ> ());
00091 fromROSMsg (*cloud_source, *xyz_source);
00092 PointCloud<PointXYZ>::Ptr xyz_target (new PointCloud<PointXYZ> ());
00093 fromROSMsg (*cloud_target, *xyz_target);
00094
00095 PointCloud<PointXYZI>::Ptr output_xyzi (new PointCloud<PointXYZI> ());
00096 output_xyzi->points.resize (xyz_source->points.size ());
00097 output_xyzi->height = cloud_source->height;
00098 output_xyzi->width = cloud_source->width;
00099
00100 float rmse = 0.0f;
00101
00102 if (correspondence_type == "index")
00103 {
00104
00105
00106 if (xyz_source->points.size () != xyz_target->points.size ())
00107 {
00108 print_error ("Source and target clouds do not have the same number of points.\n");
00109 return;
00110 }
00111
00112 for (size_t point_i = 0; point_i < xyz_source->points.size (); ++point_i)
00113 {
00114 if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
00115 continue;
00116 if (!pcl_isfinite (xyz_target->points[point_i].x) || !pcl_isfinite (xyz_target->points[point_i].y) || !pcl_isfinite (xyz_target->points[point_i].z))
00117 continue;
00118
00119
00120 float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_i]);
00121 rmse += dist;
00122
00123 output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
00124 output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
00125 output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
00126 output_xyzi->points[point_i].intensity = dist;
00127 }
00128 rmse = sqrtf (rmse / static_cast<float> (xyz_source->points.size ()));
00129 }
00130 else if (correspondence_type == "nn")
00131 {
00132
00133
00134 KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
00135 tree->setInputCloud (xyz_target);
00136
00137 for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
00138 {
00139 if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
00140 continue;
00141
00142 std::vector<int> nn_indices (1);
00143 std::vector<float> nn_distances (1);
00144 if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
00145 continue;
00146 size_t point_nn_i = nn_indices.front();
00147
00148 float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_nn_i]);
00149 rmse += dist;
00150
00151 output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
00152 output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
00153 output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
00154 output_xyzi->points[point_i].intensity = dist;
00155 }
00156 rmse = sqrtf (rmse / static_cast<float> (xyz_source->points.size ()));
00157
00158 }
00159 else if (correspondence_type == "nnplane")
00160 {
00161
00162
00163 PointCloud<Normal>::Ptr normals_target (new PointCloud<Normal> ());
00164 fromROSMsg (*cloud_target, *normals_target);
00165
00166 KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
00167 tree->setInputCloud (xyz_target);
00168
00169 for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
00170 {
00171 if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
00172 continue;
00173
00174 std::vector<int> nn_indices (1);
00175 std::vector<float> nn_distances (1);
00176 if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
00177 continue;
00178 size_t point_nn_i = nn_indices.front();
00179
00180 Eigen::Vector3f normal_target = normals_target->points[point_nn_i].getNormalVector3fMap (),
00181 point_source = xyz_source->points[point_i].getVector3fMap (),
00182 point_target = xyz_target->points[point_nn_i].getVector3fMap ();
00183
00184 float dist = normal_target.dot (point_source - point_target);
00185 rmse += dist * dist;
00186
00187 output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
00188 output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
00189 output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
00190 output_xyzi->points[point_i].intensity = dist * dist;
00191 }
00192 rmse = sqrtf (rmse / static_cast<float> (xyz_source->points.size ()));
00193 }
00194 else
00195 {
00196
00197 return;
00198 }
00199
00200 toROSMsg (*output_xyzi, output);
00201
00202
00203 print_highlight ("RMSE Error: %f\n", rmse);
00204 }
00205
00206 void
00207 saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
00208 {
00209 TicToc tt;
00210 tt.tic ();
00211
00212
00213
00214 pcl::io::savePCDFile (filename, output);
00215
00216
00217 }
00218
00219
00220 int
00221 main (int argc, char** argv)
00222 {
00223
00224
00225 if (argc < 4)
00226 {
00227 printHelp (argc, argv);
00228 return (-1);
00229 }
00230
00231
00232 std::vector<int> p_file_indices;
00233 p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00234 if (p_file_indices.size () != 3)
00235 {
00236 print_error ("Need two input PCD files and one output PCD file to continue.\n");
00237 return (-1);
00238 }
00239
00240
00241 std::string correspondence_type = default_correspondence_type;
00242 parse_argument (argc, argv, "-correspondence", correspondence_type);
00243
00244
00245 sensor_msgs::PointCloud2::Ptr cloud_source (new sensor_msgs::PointCloud2 ());
00246 if (!loadCloud (argv[p_file_indices[0]], *cloud_source))
00247 return (-1);
00248
00249 sensor_msgs::PointCloud2::Ptr cloud_target (new sensor_msgs::PointCloud2 ());
00250 if (!loadCloud (argv[p_file_indices[1]], *cloud_target))
00251 return (-1);
00252
00253 sensor_msgs::PointCloud2 output;
00254
00255 compute (cloud_source, cloud_target, output, correspondence_type);
00256
00257
00258 saveCloud (argv[p_file_indices[2]], output);
00259 }