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) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id: lum.hpp 5663 2012-05-02 13:49:39Z florentinus $ 00038 * 00039 */ 00040 00041 #ifndef PCL_REGISTRATION_IMPL_LUM_HPP_ 00042 #define PCL_REGISTRATION_IMPL_LUM_HPP_ 00043 00044 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00045 template<typename PointT> inline void 00046 pcl::registration::LUM<PointT>::setLoopGraph (const SLAMGraphPtr &slam_graph) 00047 { 00048 slam_graph_ = slam_graph; 00049 } 00050 00051 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00052 template<typename PointT> inline typename pcl::registration::LUM<PointT>::SLAMGraphPtr 00053 pcl::registration::LUM<PointT>::getLoopGraph () const 00054 { 00055 return (slam_graph_); 00056 } 00057 00058 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00059 template<typename PointT> typename pcl::registration::LUM<PointT>::SLAMGraph::vertices_size_type 00060 pcl::registration::LUM<PointT>::getNumVertices () const 00061 { 00062 return (num_vertices (*slam_graph_)); 00063 } 00064 00065 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00066 template<typename PointT> void 00067 pcl::registration::LUM<PointT>::setMaxIterations (int max_iterations) 00068 { 00069 max_iterations_ = max_iterations; 00070 } 00071 00072 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00073 template<typename PointT> inline int 00074 pcl::registration::LUM<PointT>::getMaxIterations () const 00075 { 00076 return (max_iterations_); 00077 } 00078 00079 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00080 template<typename PointT> void 00081 pcl::registration::LUM<PointT>::setConvergenceThreshold (float convergence_threshold) 00082 { 00083 convergence_threshold_ = convergence_threshold; 00084 } 00085 00086 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00087 template<typename PointT> inline float 00088 pcl::registration::LUM<PointT>::getConvergenceThreshold () const 00089 { 00090 return (convergence_threshold_); 00091 } 00092 00093 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00094 template<typename PointT> typename pcl::registration::LUM<PointT>::Vertex 00095 pcl::registration::LUM<PointT>::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose) 00096 { 00097 Vertex v = add_vertex (*slam_graph_); 00098 (*slam_graph_)[v].cloud_ = cloud; 00099 if (v == 0 && pose != Eigen::Vector6f::Zero ()) 00100 { 00101 PCL_WARN("[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n"); 00102 (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero (); 00103 return (v); 00104 } 00105 (*slam_graph_)[v].pose_ = pose; 00106 return (v); 00107 } 00108 00109 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00110 template<typename PointT> inline void 00111 pcl::registration::LUM<PointT>::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud) 00112 { 00113 if (vertex >= getNumVertices ()) 00114 { 00115 PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n"); 00116 return; 00117 } 00118 (*slam_graph_)[vertex].cloud_ = cloud; 00119 } 00120 00121 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00122 template<typename PointT> inline typename pcl::registration::LUM<PointT>::PointCloudPtr 00123 pcl::registration::LUM<PointT>::getPointCloud (const Vertex &vertex) const 00124 { 00125 if (vertex >= getNumVertices ()) 00126 { 00127 PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n"); 00128 return (PointCloudPtr ()); 00129 } 00130 return ((*slam_graph_)[vertex].cloud_); 00131 } 00132 00133 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00134 template<typename PointT> inline void 00135 pcl::registration::LUM<PointT>::setPose (const Vertex &vertex, const Eigen::Vector6f &pose) 00136 { 00137 if (vertex >= getNumVertices ()) 00138 { 00139 PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n"); 00140 return; 00141 } 00142 if (vertex == 0) 00143 { 00144 PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n"); 00145 return; 00146 } 00147 (*slam_graph_)[vertex].pose_ = pose; 00148 } 00149 00150 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00151 template<typename PointT> inline Eigen::Vector6f 00152 pcl::registration::LUM<PointT>::getPose (const Vertex &vertex) const 00153 { 00154 if (vertex >= getNumVertices ()) 00155 { 00156 PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n"); 00157 return (Eigen::Vector6f::Zero ()); 00158 } 00159 return ((*slam_graph_)[vertex].pose_); 00160 } 00161 00162 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00163 template<typename PointT> inline Eigen::Affine3f 00164 pcl::registration::LUM<PointT>::getTransformation (const Vertex &vertex) const 00165 { 00166 Eigen::Vector6f pose = getPose (vertex); 00167 return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5))); 00168 } 00169 00170 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00171 template<typename PointT> void 00172 pcl::registration::LUM<PointT>::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs) 00173 { 00174 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex) 00175 { 00176 PCL_ERROR("[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n"); 00177 return; 00178 } 00179 Edge e; 00180 bool present; 00181 boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_); 00182 if (!present) 00183 boost::tuples::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_); 00184 (*slam_graph_)[e].corrs_ = corrs; 00185 } 00186 00187 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00188 template<typename PointT> inline pcl::CorrespondencesPtr 00189 pcl::registration::LUM<PointT>::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const 00190 { 00191 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ()) 00192 { 00193 PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n"); 00194 return (pcl::CorrespondencesPtr ()); 00195 } 00196 Edge e; 00197 bool present; 00198 boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_); 00199 if (!present) 00200 { 00201 PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n"); 00202 return (pcl::CorrespondencesPtr ()); 00203 } 00204 return ((*slam_graph_)[e].corrs_); 00205 } 00206 00207 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00208 template<typename PointT> void 00209 pcl::registration::LUM<PointT>::compute () 00210 { 00211 int n = static_cast<int> (getNumVertices ()); 00212 if (n < 2) 00213 { 00214 PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n"); 00215 return; 00216 } 00217 for (int i = 0; i < max_iterations_; ++i) 00218 { 00219 // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_) 00220 typename SLAMGraph::edge_iterator e, e_end; 00221 for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e) 00222 computeEdge (*e); 00223 00224 // Declare matrices G and B 00225 Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1)); 00226 Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1)); 00227 00228 // Start at 1 because 0 is the reference pose 00229 for (int vi = 1; vi != n; ++vi) 00230 { 00231 for (int vj = 0; vj != n; ++vj) 00232 { 00233 // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge 00234 Edge e; 00235 bool present1, present2; 00236 boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_); 00237 if (!present1) 00238 { 00239 boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_); 00240 if (!present2) 00241 continue; 00242 } 00243 00244 // Fill in elements of G and B 00245 if (vj > 0) 00246 G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_; 00247 G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_; 00248 B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_; 00249 } 00250 } 00251 00252 // Computation of the linear equation system: GX = B 00253 // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse) 00254 Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B); 00255 00256 // Update the poses 00257 float sum = 0.0; 00258 for (int vi = 1; vi != n; ++vi) 00259 { 00260 Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6)); 00261 sum += difference_pose.norm (); 00262 setPose (vi, getPose (vi) + difference_pose); 00263 } 00264 00265 // Convergence check 00266 if (sum <= convergence_threshold_ * static_cast<float> (n - 1)) 00267 return; 00268 } 00269 } 00270 00271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00272 template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr 00273 pcl::registration::LUM<PointT>::getTransformedCloud (const Vertex &vertex) const 00274 { 00275 PointCloudPtr out (new PointCloud); 00276 pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex)); 00277 return (out); 00278 } 00279 00280 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00281 template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr 00282 pcl::registration::LUM<PointT>::getConcatenatedCloud () const 00283 { 00284 PointCloudPtr out (new PointCloud); 00285 typename SLAMGraph::vertex_iterator v, v_end; 00286 for (boost::tuples::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v) 00287 { 00288 PointCloud temp; 00289 pcl::transformPointCloud (*getPointCloud (*v), temp, getTransformation (*v)); 00290 *out += temp; 00291 } 00292 return (out); 00293 } 00294 00295 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00296 template<typename PointT> void 00297 pcl::registration::LUM<PointT>::computeEdge (const Edge &e) 00298 { 00299 // Get necessary local data from graph 00300 PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_; 00301 PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_; 00302 Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_; 00303 Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_; 00304 pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_; 00305 00306 // Build the average and difference vectors for all correspondences 00307 std::vector <Eigen::Vector3f> corrs_aver (corrs->size ()); 00308 std::vector <Eigen::Vector3f> corrs_diff (corrs->size ()); 00309 int oci = 0; // oci = output correspondence iterator 00310 for (int ici = 0; ici != static_cast<int> (corrs->size ()); ++ici) // ici = input correspondence iterator 00311 { 00312 // Compound the point pair onto the current pose 00313 Eigen::Vector3f source_compounded = pcl::getTransformation (source_pose (0), source_pose (1), source_pose (2), source_pose (3), source_pose (4), source_pose (5)) * source_cloud->points[(*corrs)[ici].index_query].getVector3fMap (); 00314 Eigen::Vector3f target_compounded = pcl::getTransformation (target_pose (0), target_pose (1), target_pose (2), target_pose (3), target_pose (4), target_pose (5)) * target_cloud->points[(*corrs)[ici].index_match].getVector3fMap (); 00315 00316 // NaN points can not be passed to the remaining computational pipeline 00317 if (!pcl_isfinite (source_compounded (0)) || !pcl_isfinite (source_compounded (1)) || !pcl_isfinite (source_compounded (2)) || !pcl_isfinite (target_compounded (0)) || !pcl_isfinite (target_compounded (1)) || !pcl_isfinite (target_compounded (2))) 00318 continue; 00319 00320 // Compute the point pair average and difference and store for later use 00321 corrs_aver[oci] = 0.5 * (source_compounded + target_compounded); 00322 corrs_diff[oci] = source_compounded - target_compounded; 00323 oci++; 00324 } 00325 corrs_aver.resize (oci); 00326 corrs_diff.resize (oci); 00327 00328 // Need enough valid correspondences to get a good triangulation 00329 if (oci < 3) 00330 { 00331 PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d and %d do not contain enough valid correspondences to be considered for computation.\n", source (e, *slam_graph_), target (e, *slam_graph_)); 00332 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero (); 00333 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero (); 00334 return; 00335 } 00336 00337 // Build the matrices M'M and M'Z 00338 Eigen::Matrix6f MM = Eigen::Matrix6f::Zero (); 00339 Eigen::Vector6f MZ = Eigen::Vector6f::Zero (); 00340 for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator 00341 { 00342 // Fast computation of summation elements of M'M 00343 MM (0, 4) -= corrs_aver[ci] (1); 00344 MM (0, 5) += corrs_aver[ci] (2); 00345 MM (1, 3) -= corrs_aver[ci] (2); 00346 MM (1, 4) += corrs_aver[ci] (0); 00347 MM (2, 3) += corrs_aver[ci] (1); 00348 MM (2, 5) -= corrs_aver[ci] (0); 00349 MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2); 00350 MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1); 00351 MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2); 00352 MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2); 00353 MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1); 00354 MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2); 00355 00356 // Fast computation of M'Z 00357 MZ (0) += corrs_diff[ci] (0); 00358 MZ (1) += corrs_diff[ci] (1); 00359 MZ (2) += corrs_diff[ci] (2); 00360 MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1); 00361 MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0); 00362 MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2); 00363 } 00364 // Remaining elements of M'M 00365 MM (0, 0) = MM (1, 1) = MM (2, 2) = static_cast<float> (oci); 00366 MM (4, 0) = MM (0, 4); 00367 MM (5, 0) = MM (0, 5); 00368 MM (3, 1) = MM (1, 3); 00369 MM (4, 1) = MM (1, 4); 00370 MM (3, 2) = MM (2, 3); 00371 MM (5, 2) = MM (2, 5); 00372 MM (4, 3) = MM (3, 4); 00373 MM (5, 3) = MM (3, 5); 00374 MM (5, 4) = MM (4, 5); 00375 00376 // Compute pose difference estimation 00377 Eigen::Vector6f D = static_cast<Eigen::Vector6f> (MM.inverse () * MZ); 00378 00379 // Compute s^2 00380 float ss = 0.0f; 00381 for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator 00382 ss += static_cast<float> (pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f) 00383 + pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f) 00384 + pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f)); 00385 00386 // When reaching the limitations of computation due to linearization 00387 if (ss < 0.0000000000001 || !pcl_isfinite (ss)) 00388 { 00389 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero (); 00390 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero (); 00391 return; 00392 } 00393 00394 // Store the results in the slam graph 00395 (*slam_graph_)[e].cinv_ = MM * (1.0f / ss); 00396 (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss); 00397 } 00398 00399 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00400 template<typename PointT> inline Eigen::Matrix6f 00401 pcl::registration::LUM<PointT>::incidenceCorrection (const Eigen::Vector6f &pose) 00402 { 00403 Eigen::Matrix6f out = Eigen::Matrix6f::Identity (); 00404 float cx = cosf (pose (3)), sx = sinf (pose (3)), cy = cosf (pose (4)), sy = sinf (pose (4)); 00405 out (0, 4) = pose (1) * sx - pose (2) * cx; 00406 out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy; 00407 out (1, 3) = pose (2); 00408 out (1, 4) = -pose (0) * sx; 00409 out (1, 5) = -pose (0) * cx * cy + pose (2) * sy; 00410 out (2, 3) = -pose (1); 00411 out (2, 4) = pose (0) * cx; 00412 out (2, 5) = -pose (0) * sx * cy - pose (1) * sy; 00413 out (3, 5) = sy; 00414 out (4, 4) = sx; 00415 out (4, 5) = cx * cy; 00416 out (5, 4) = cx; 00417 out (5, 5) = -sx * cy; 00418 return (out); 00419 } 00420 00421 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>; 00422 00423 #endif // PCL_REGISTRATION_IMPL_LUM_HPP_ 00424