icp2d.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #include <pcl/console/parse.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl/registration/icp.h>
00044 #include <pcl/registration/icp_nl.h>
00045 #include <pcl/registration/transformation_estimation_lm.h>
00046 #include <pcl/registration/warp_point_rigid_3d.h>
00047 
00048 #include <string>
00049 #include <iostream>
00050 #include <fstream>
00051 #include <vector>
00052 
00053 typedef pcl::PointXYZ PointType;
00054 typedef pcl::PointCloud<PointType> Cloud;
00055 typedef Cloud::ConstPtr CloudConstPtr;
00056 typedef Cloud::Ptr CloudPtr;
00057 
00058 int
00059 main (int argc, char **argv)
00060 {
00061   double dist = 0.05;
00062   pcl::console::parse_argument (argc, argv, "-d", dist);
00063 
00064   double rans = 0.05;
00065   pcl::console::parse_argument (argc, argv, "-r", rans);
00066 
00067   int iter = 50;
00068   pcl::console::parse_argument (argc, argv, "-i", iter);
00069 
00070   std::vector<int> pcd_indices;
00071   pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
00072 
00073   CloudPtr model (new Cloud);
00074   if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
00075   {
00076     std::cout << "Could not read file" << std::endl;
00077     return -1;
00078   }
00079   std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
00080 
00081   std::string result_filename (argv[pcd_indices[0]]);
00082   result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
00083   pcl::io::savePCDFile (result_filename.c_str (), *model);
00084   std::cout << "saving first model to " << result_filename << std::endl;
00085 
00086   Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
00087 
00088   for (size_t i = 1; i < pcd_indices.size (); i++)
00089   {
00090     CloudPtr data (new Cloud);
00091     if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
00092     {
00093       std::cout << "Could not read file" << std::endl;
00094       return -1;
00095     }
00096     std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
00097 
00098     pcl::IterativeClosestPointNonLinear<PointType, PointType> icp;
00099 
00100     boost::shared_ptr<pcl::registration::WarpPointRigid3D<PointType, PointType> > warp_fcn 
00101       (new pcl::registration::WarpPointRigid3D<PointType, PointType>);
00102 
00103     // Create a TransformationEstimationLM object, and set the warp to it
00104     boost::shared_ptr<pcl::registration::TransformationEstimationLM<PointType, PointType> > te (new pcl::registration::TransformationEstimationLM<PointType, PointType>);
00105     te->setWarpFunction (warp_fcn);
00106 
00107     // Pass the TransformationEstimation objec to the ICP algorithm
00108     icp.setTransformationEstimation (te);
00109 
00110     icp.setMaximumIterations (iter);
00111     icp.setMaxCorrespondenceDistance (dist);
00112     icp.setRANSACOutlierRejectionThreshold (rans);
00113 
00114     icp.setInputTarget (model);
00115 
00116     icp.setInputSource (data);
00117 
00118     CloudPtr tmp (new Cloud);
00119     icp.align (*tmp);
00120 
00121     t = t * icp.getFinalTransformation ();
00122 
00123     pcl::transformPointCloud (*data, *tmp, t);
00124 
00125     std::cout << icp.getFinalTransformation () << std::endl;
00126 
00127     *model = *data;
00128 
00129     std::string result_filename (argv[pcd_indices[i]]);
00130     result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
00131     pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
00132     std::cout << "saving result to " << result_filename << std::endl;
00133   }
00134 
00135   return 0;
00136 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:53