registration_visualizer.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: registration_visualizer.cpp 2011-07-24 01:06:01Z georgeLisca $
00035  *
00036  */
00037 
00038 // PCL
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/filters/voxel_grid.h>
00041 #include <pcl/registration/icp.h>
00042 #include <pcl/registration/icp_nl.h>
00043 #include <pcl/visualization/registration_visualizer.h>
00044 
00045 int
00046 main (int argc, char** argv)
00047 {
00048 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00049 
00050   if (argc != 3)
00051   {
00052     std::cerr << "please specify the paths to the two point clouds to be registered" << std::endl;
00053     exit (0);
00054   }
00055 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00056 
00057 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00058   pcl::PointCloud<pcl::PointXYZ> inputCloud;
00059   pcl::PointCloud<pcl::PointXYZ> targetCloud;
00060 
00061   if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], inputCloud) == -1) //* load the file
00062   {
00063     PCL_ERROR ("Couldn't read the first .pcd file \n");
00064     return (-1);
00065   }
00066 
00067   if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[2], targetCloud) == -1) //* load the file
00068   {
00069     PCL_ERROR ("Couldn't read the second .pcd file \n");
00070     return (-1);
00071   }
00072 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00073 
00074 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00075   pcl::PointCloud<pcl::PointXYZ> inputCloudFiltered;
00076   pcl::PointCloud<pcl::PointXYZ> targetCloudFiltered;
00077 
00078   pcl::VoxelGrid<pcl::PointXYZ> sor;
00079 //  sor.setLeafSize (0.01, 0.01, 0.01);
00080   sor.setLeafSize (0.02f, 0.02f, 0.02f);
00081 //  sor.setLeafSize (0.05, 0.05, 0.05);
00082 //  sor.setLeafSize (0.1, 0.1, 0.1);
00083 //  sor.setLeafSize (0.4, 0.4, 0.4);
00084 //  sor.setLeafSize (0.5, 0.5, 0.5);
00085 
00086   sor.setInputCloud (inputCloud.makeShared());
00087   std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl;
00088   sor.filter (inputCloudFiltered);
00089   std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl;
00090 
00091   sor.setInputCloud (targetCloud.makeShared());
00092   std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl;
00093   sor.filter (targetCloudFiltered);
00094   std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl;
00095 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00096 
00097 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00098 //  pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloud.makeShared();
00099 //  pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloud.makeShared();
00100 
00101   pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloudFiltered.makeShared();
00102   pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloudFiltered.makeShared();
00103 
00104   pcl::PointCloud<pcl::PointXYZ> source_aligned;
00105 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00106 
00107 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00108   pcl::RegistrationVisualizer<pcl::PointXYZ, pcl::PointXYZ> registrationVisualizer;
00109 
00110   registrationVisualizer.startDisplay();
00111 
00112   registrationVisualizer.setMaximumDisplayedCorrespondences (100);
00113 //  /////////////////////////////////////////////////////////////////////////////////////////////////////
00114 
00115 //  ///////////////////////////////////////////////////////////////////////////////////////////////////
00116   pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
00117 
00118   icp.setMaximumIterations(10000);
00119 
00120 //  icp.setMaxCorrespondenceDistance (0.6);
00121   icp.setMaxCorrespondenceDistance (0.8);
00122 //  icp.setMaxCorrespondenceDistance (1.5);
00123 
00124 //  icp.setRANSACOutlierRejectionThreshold (0.1);
00125   icp.setRANSACOutlierRejectionThreshold (0.6);
00126 //  icp.setRANSACOutlierRejectionThreshold (1.5);
00127 //  icp.setRANSACOutlierRejectionThreshold (5.0);
00128 
00129   icp.setInputCloud  (source);
00130   icp.setInputTarget (target);
00131 
00132   // Register the registration algorithm to the RegistrationVisualizer
00133   registrationVisualizer.setRegistration (icp);
00134 
00135   // Start registration process
00136   icp.align (source_aligned);
00137 
00138   std::cout << "has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl;
00139   std::cout << icp.getFinalTransformation () << std::endl;
00140 //  ///////////////////////////////////////////////////////////////////////////////////////////////////
00141 
00142 //  ///////////////////////////////////////////////////////////////////////////////////////////////////
00143 //
00144 //  pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icpnl;
00145 //
00146 //  icpnl.setMaximumIterations(10000);
00147 //
00148 //  icpnl.setMaxCorrespondenceDistance (0.6);
00151 //
00153 //  icpnl.setRANSACOutlierRejectionThreshold (0.8);
00156 //
00157 //  icpnl.setInputCloud  (source);
00158 //  icpnl.setInputTarget (target);
00159 //
00160 //  // Register the registration algorithm to the RegistrationVisualizer
00161 //  registrationVisualizer.setRegistration (icpnl);
00162 //
00163 //  // Start registration process
00164 //  icpnl.align (source_aligned);
00165 //
00166 //  std::cout << "has converged:" << icpnl.hasConverged () << " score: " << icpnl.getFitnessScore () << std::endl;
00167 //  std::cout << icpnl.getFinalTransformation () << std::endl;
00168 //
00169 //  ///////////////////////////////////////////////////////////////////////////////////////////////////
00170 
00171 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:38