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 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ 00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ 00040 00041 #include <pcl/sample_consensus/sac_model_registration_2d.h> 00042 #include <pcl/common/point_operators.h> 00043 #include <pcl/common/eigen.h> 00044 00045 ////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> bool 00047 pcl::SampleConsensusModelRegistration2D<PointT>::isSampleGood (const std::vector<int> &samples) const 00048 { 00049 return (true); 00050 //using namespace pcl::common; 00051 //using namespace pcl::traits; 00052 00053 //PointT p10 = input_->points[samples[1]] - input_->points[samples[0]]; 00054 //PointT p20 = input_->points[samples[2]] - input_->points[samples[0]]; 00055 //PointT p21 = input_->points[samples[2]] - input_->points[samples[1]]; 00056 00057 //return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ && 00058 // (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ && 00059 // (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_); 00060 } 00061 00062 ////////////////////////////////////////////////////////////////////////// 00063 template <typename PointT> void 00064 pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00065 { 00066 PCL_INFO ("[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n"); 00067 if (indices_->size () != indices_tgt_->size ()) 00068 { 00069 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); 00070 distances.clear (); 00071 return; 00072 } 00073 if (!target_) 00074 { 00075 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n"); 00076 return; 00077 } 00078 00079 distances.resize (indices_->size ()); 00080 00081 // Get the 4x4 transformation 00082 Eigen::Matrix4f transform; 00083 transform.row (0).matrix () = model_coefficients.segment<4>(0); 00084 transform.row (1).matrix () = model_coefficients.segment<4>(4); 00085 transform.row (2).matrix () = model_coefficients.segment<4>(8); 00086 transform.row (3).matrix () = model_coefficients.segment<4>(12); 00087 00088 for (size_t i = 0; i < indices_->size (); ++i) 00089 { 00090 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, 00091 input_->points[(*indices_)[i]].y, 00092 input_->points[(*indices_)[i]].z, 1); 00093 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, 00094 target_->points[(*indices_tgt_)[i]].y, 00095 target_->points[(*indices_tgt_)[i]].z, 1); 00096 00097 Eigen::Vector4f p_tr (transform * pt_src); 00098 00099 // Project the point on the image plane 00100 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); 00101 Eigen::Vector3f uv (projection_matrix_ * p_tr3); 00102 00103 if (uv[2] < 0) 00104 continue; 00105 00106 uv /= uv[2]; 00107 00108 // Calculate the distance from the transformed point to its correspondence 00109 // need to compute the real norm here to keep MSAC and friends general 00110 distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) * 00111 (uv[0] - target_->points[(*indices_tgt_)[i]].u) + 00112 (uv[1] - target_->points[(*indices_tgt_)[i]].v) * 00113 (uv[1] - target_->points[(*indices_tgt_)[i]].v)); 00114 } 00115 } 00116 00117 ////////////////////////////////////////////////////////////////////////// 00118 template <typename PointT> void 00119 pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00120 { 00121 if (indices_->size () != indices_tgt_->size ()) 00122 { 00123 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); 00124 inliers.clear (); 00125 return; 00126 } 00127 if (!target_) 00128 { 00129 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n"); 00130 return; 00131 } 00132 00133 double thresh = threshold * threshold; 00134 00135 int nr_p = 0; 00136 inliers.resize (indices_->size ()); 00137 error_sqr_dists_.resize (indices_->size ()); 00138 00139 Eigen::Matrix4f transform; 00140 transform.row (0).matrix () = model_coefficients.segment<4>(0); 00141 transform.row (1).matrix () = model_coefficients.segment<4>(4); 00142 transform.row (2).matrix () = model_coefficients.segment<4>(8); 00143 transform.row (3).matrix () = model_coefficients.segment<4>(12); 00144 00145 for (size_t i = 0; i < indices_->size (); ++i) 00146 { 00147 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, 00148 input_->points[(*indices_)[i]].y, 00149 input_->points[(*indices_)[i]].z, 1); 00150 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, 00151 target_->points[(*indices_tgt_)[i]].y, 00152 target_->points[(*indices_tgt_)[i]].z, 1); 00153 00154 Eigen::Vector4f p_tr (transform * pt_src); 00155 00156 // Project the point on the image plane 00157 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); 00158 Eigen::Vector3f uv (projection_matrix_ * p_tr3); 00159 00160 if (uv[2] < 0) 00161 continue; 00162 00163 uv /= uv[2]; 00164 00165 double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) * 00166 (uv[0] - target_->points[(*indices_tgt_)[i]].u) + 00167 (uv[1] - target_->points[(*indices_tgt_)[i]].v) * 00168 (uv[1] - target_->points[(*indices_tgt_)[i]].v)); 00169 00170 // Calculate the distance from the transformed point to its correspondence 00171 if (distance < thresh) 00172 { 00173 inliers[nr_p] = (*indices_)[i]; 00174 error_sqr_dists_[nr_p] = distance; 00175 ++nr_p; 00176 } 00177 } 00178 inliers.resize (nr_p); 00179 error_sqr_dists_.resize (nr_p); 00180 } 00181 00182 ////////////////////////////////////////////////////////////////////////// 00183 template <typename PointT> int 00184 pcl::SampleConsensusModelRegistration2D<PointT>::countWithinDistance ( 00185 const Eigen::VectorXf &model_coefficients, const double threshold) 00186 { 00187 if (indices_->size () != indices_tgt_->size ()) 00188 { 00189 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); 00190 return (0); 00191 } 00192 if (!target_) 00193 { 00194 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n"); 00195 return (0); 00196 } 00197 00198 double thresh = threshold * threshold; 00199 00200 Eigen::Matrix4f transform; 00201 transform.row (0).matrix () = model_coefficients.segment<4>(0); 00202 transform.row (1).matrix () = model_coefficients.segment<4>(4); 00203 transform.row (2).matrix () = model_coefficients.segment<4>(8); 00204 transform.row (3).matrix () = model_coefficients.segment<4>(12); 00205 00206 int nr_p = 0; 00207 00208 for (size_t i = 0; i < indices_->size (); ++i) 00209 { 00210 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, 00211 input_->points[(*indices_)[i]].y, 00212 input_->points[(*indices_)[i]].z, 1); 00213 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, 00214 target_->points[(*indices_tgt_)[i]].y, 00215 target_->points[(*indices_tgt_)[i]].z, 1); 00216 00217 Eigen::Vector4f p_tr (transform * pt_src); 00218 00219 // Project the point on the image plane 00220 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); 00221 Eigen::Vector3f uv (projection_matrix_ * p_tr3); 00222 00223 if (uv[2] < 0) 00224 continue; 00225 00226 uv /= uv[2]; 00227 00228 // Calculate the distance from the transformed point to its correspondence 00229 if (((uv[0] - target_->points[(*indices_tgt_)[i]].u) * 00230 (uv[0] - target_->points[(*indices_tgt_)[i]].u) + 00231 (uv[1] - target_->points[(*indices_tgt_)[i]].v) * 00232 (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh) 00233 ++nr_p; 00234 } 00235 return (nr_p); 00236 } 00237 00238 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ 00239