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 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ 00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ 00041 00042 ////////////////////////////////////////////////////////////////////////////////////////////// 00043 template <typename PointSource, typename PointTarget, typename Scalar> void 00044 pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation ( 00045 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean, 00046 const Eigen::Matrix<Scalar, 4, 1> ¢roid_src, 00047 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean, 00048 const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt, 00049 Matrix4 &transformation_matrix) const 00050 { 00051 transformation_matrix.setIdentity (); 00052 00053 // Assemble the correlation matrix H = source * target' 00054 Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); 00055 00056 // Compute the Singular Value Decomposition 00057 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); 00058 Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU (); 00059 Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV (); 00060 00061 // Compute R = V * U' 00062 if (u.determinant () * v.determinant () < 0) 00063 { 00064 for (int x = 0; x < 3; ++x) 00065 v (x, 2) *= -1; 00066 } 00067 00068 Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose (); 00069 00070 // rotated cloud 00071 Eigen::Matrix<Scalar, 4, 4> R4; 00072 R4.block (0, 0, 3, 3) = R; 00073 R4 (0, 3) = 0; 00074 R4 (1, 3) = 0; 00075 R4 (2, 3) = 0; 00076 R4 (3, 3) = 1; 00077 00078 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R4 * cloud_src_demean; 00079 00080 float scale1, scale2; 00081 double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f; 00082 for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx) 00083 { 00084 sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx); 00085 sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx); 00086 sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx); 00087 00088 sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx); 00089 sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx); 00090 sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx); 00091 00092 sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx); 00093 sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx); 00094 sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx); 00095 } 00096 00097 scale1 = sqrt (sum_tt / sum_ss); 00098 scale2 = sum_tt_ / sum_ss; 00099 float scale = scale2; 00100 transformation_matrix.topLeftCorner (3, 3) = scale * R; 00101 const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3)); 00102 transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc; 00103 } 00104 00105 //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>; 00106 00107 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ */