Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, 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 #ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ 00041 #define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ 00042 00043 #include <pcl/console/print.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename Scalar> bool 00047 pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged () 00048 { 00049 convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED; 00050 00051 PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_); 00052 // 1. Number of iterations has reached the maximum user imposed number of iterations 00053 if (iterations_ >= max_iterations_) 00054 { 00055 if (failure_after_max_iter_) 00056 return (false); 00057 else 00058 { 00059 convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS; 00060 return (true); 00061 } 00062 return (failure_after_max_iter_ ? false : true); 00063 } 00064 00065 // 2. The epsilon (difference) between the previous transformation and the current estimated transformation 00066 double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1); 00067 double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) + 00068 transformation_.coeff (1, 3) * transformation_.coeff (1, 3) + 00069 transformation_.coeff (2, 3) * transformation_.coeff (2, 3); 00070 PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave %f rotation (cosine) and %f translation.\n", cos_angle, translation_sqr); 00071 00072 if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_) 00073 { 00074 if (iterations_similar_transforms_ < max_iterations_similar_transforms_) 00075 { 00076 // Increment the number of transforms that the thresholds are allowed to be similar 00077 ++iterations_similar_transforms_; 00078 return (false); 00079 } 00080 else 00081 { 00082 iterations_similar_transforms_ = 0; 00083 convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM; 00084 return (true); 00085 } 00086 } 00087 00088 correspondences_cur_mse_ = calculateMSE (correspondences_); 00089 PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: %f / %f.\n", correspondences_prev_mse_, correspondences_cur_mse_); 00090 00091 // 3. The relative sum of Euclidean squared errors is smaller than a user defined threshold 00092 // Absolute 00093 if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_) 00094 { 00095 if (iterations_similar_transforms_ < max_iterations_similar_transforms_) 00096 { 00097 // Increment the number of transforms that the thresholds are allowed to be similar 00098 ++iterations_similar_transforms_; 00099 return (false); 00100 } 00101 else 00102 { 00103 iterations_similar_transforms_ = 0; 00104 convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE; 00105 return (true); 00106 } 00107 } 00108 00109 // Relative 00110 if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_) 00111 { 00112 if (iterations_similar_transforms_ < max_iterations_similar_transforms_) 00113 { 00114 // Increment the number of transforms that the thresholds are allowed to be similar 00115 ++iterations_similar_transforms_; 00116 return (false); 00117 } 00118 else 00119 { 00120 iterations_similar_transforms_ = 0; 00121 convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE; 00122 return (true); 00123 } 00124 } 00125 00126 correspondences_prev_mse_ = correspondences_cur_mse_; 00127 00128 return (false); 00129 } 00130 00131 #endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_