registration_correspondences_check.hpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Based on original code written by Radu Rusu
00032  * Modified by Hozefa Indorewala
00033  */
00034 
00036 
00039 template <typename PointSource, typename PointTarget> inline void
00040   pcl::RegistrationCorrespondencesCheck<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
00041 {
00042   if (cloud->points.empty ())
00043   {
00044     ROS_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!", getClassName ().c_str ());
00045     return;
00046   }
00047   target_ = cloud;
00048   tree_->setInputCloud (target_);
00049 }
00050 
00052 
00055 template <typename PointSource, typename PointTarget> inline double
00056   pcl::RegistrationCorrespondencesCheck<PointSource, PointTarget>::getFitnessScore (double max_range)
00057 {
00058   double fitness_score = 0.0;
00059 
00060   // Transform the input dataset using the final transformation
00061   PointCloudSource input_transformed;
00062   transformPointCloud (*input_, input_transformed, final_transformation_);
00063 
00064   std::vector<int> nn_indices (1);
00065   std::vector<float> nn_dists (1);
00066 
00067   // For each point in the source dataset
00068   int nr = 0;
00069   for (size_t i = 0; i < input_transformed.points.size (); ++i)
00070   {
00071     Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x,
00072                                           input_transformed.points[i].y,
00073                                           input_transformed.points[i].z, 0);
00074     // Find its nearest neighbor in the target
00075     tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
00076 
00077     // Deal with occlusions (incomplete targets)
00078     if (nn_dists[0] > max_range)
00079       continue;
00080 
00081     Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x,
00082                                           target_->points[nn_indices[0]].y,
00083                                           target_->points[nn_indices[0]].z, 0);
00084     // Calculate the fitness score
00085     fitness_score += fabs ((p1-p2).squaredNorm ());
00086     nr++;
00087   }
00088 
00089   if (nr > 0)
00090     return (fitness_score / nr);
00091   else
00092     return (std::numeric_limits<double>::max ());
00093 }
00094 
00096 
00099 template <typename PointSource, typename PointTarget> inline void
00100   pcl::RegistrationCorrespondencesCheck<PointSource, PointTarget>::align (PointCloudSource &output)
00101 {
00102   if (!initCompute ()) return;
00103 
00104   if (!target_)
00105   {
00106     ROS_WARN ("[pcl::%s::compute] No input target dataset was given!", getClassName ().c_str ());
00107     return;
00108   }
00109 
00110   // Resize the output dataset
00111   if (output.points.size () != indices_->size ())
00112     output.points.resize (indices_->size ());
00113   // Copy the header
00114   output.header   = input_->header;
00115   // Check if the output is dense or not
00116   if (indices_->size () != input_->points.size ())
00117   {
00118     output.width    = indices_->size ();
00119     output.height   = 1;
00120     output.is_dense = false;
00121   }
00122   else
00123   {
00124     output.width    = input_->width;
00125     output.height   = input_->height;
00126     output.is_dense = input_->is_dense;
00127   }
00128 
00129   // Copy the point data to output
00130   for (size_t i = 0; i < indices_->size (); ++i)
00131     output.points[i] = input_->points[(*indices_)[i]];
00132 
00133   // Perform the actual transformation computation
00134   converged_ = false;
00135   final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity ();
00136   ROS_INFO("[RegistrationCorrespondencesCheck:] computeTransformation");
00137   computeTransformation (output);
00138 
00139   deinitCompute ();
00140 }
00141 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pointcloud_registration
Author(s): Hozefa Indorewala
autogenerated on Sun Oct 6 2013 11:55:57