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_ORGANIZED_PROJECTION_MATRIX_HPP__ 00039 #define __PCL_ORGANIZED_PROJECTION_MATRIX_HPP__ 00040 00041 #include <pcl/cloud_iterator.h> 00042 00043 /////////////////////////////////////////////////////////////////////////////////////////// 00044 namespace pcl 00045 { 00046 namespace common 00047 { 00048 namespace internal 00049 { 00050 template <typename MatrixT> void 00051 makeSymmetric (MatrixT& matrix, bool use_upper_triangular = true) 00052 { 00053 if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit)) 00054 { 00055 matrix.coeffRef (4) = matrix.coeff (1); 00056 matrix.coeffRef (8) = matrix.coeff (2); 00057 matrix.coeffRef (9) = matrix.coeff (6); 00058 matrix.coeffRef (12) = matrix.coeff (3); 00059 matrix.coeffRef (13) = matrix.coeff (7); 00060 matrix.coeffRef (14) = matrix.coeff (11); 00061 } 00062 else 00063 { 00064 matrix.coeffRef (1) = matrix.coeff (4); 00065 matrix.coeffRef (2) = matrix.coeff (8); 00066 matrix.coeffRef (6) = matrix.coeff (9); 00067 matrix.coeffRef (3) = matrix.coeff (12); 00068 matrix.coeffRef (7) = matrix.coeff (13); 00069 matrix.coeffRef (11) = matrix.coeff (14); 00070 } 00071 } 00072 } 00073 } 00074 } 00075 00076 ////////////////////////////////////////////////////////////////////////////// 00077 template <typename PointT> double 00078 pcl::estimateProjectionMatrix ( 00079 typename pcl::PointCloud<PointT>::ConstPtr cloud, 00080 Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 00081 const std::vector<int>& indices) 00082 { 00083 // internally we calculate with double but store the result into float matrices. 00084 typedef double Scalar; 00085 projection_matrix.setZero (); 00086 if (cloud->height == 1 || cloud->width == 1) 00087 { 00088 PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n"); 00089 return (-1.0); 00090 } 00091 00092 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); 00093 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); 00094 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); 00095 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); 00096 00097 pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices); 00098 00099 while (pointIt) 00100 { 00101 unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width; 00102 unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width; 00103 00104 const PointT& point = *pointIt; 00105 if (pcl_isfinite (point.x)) 00106 { 00107 Scalar xx = point.x * point.x; 00108 Scalar xy = point.x * point.y; 00109 Scalar xz = point.x * point.z; 00110 Scalar yy = point.y * point.y; 00111 Scalar yz = point.y * point.z; 00112 Scalar zz = point.z * point.z; 00113 Scalar xx_yy = xIdx * xIdx + yIdx * yIdx; 00114 00115 A.coeffRef (0) += xx; 00116 A.coeffRef (1) += xy; 00117 A.coeffRef (2) += xz; 00118 A.coeffRef (3) += point.x; 00119 00120 A.coeffRef (5) += yy; 00121 A.coeffRef (6) += yz; 00122 A.coeffRef (7) += point.y; 00123 00124 A.coeffRef (10) += zz; 00125 A.coeffRef (11) += point.z; 00126 A.coeffRef (15) += 1.0; 00127 00128 B.coeffRef (0) -= xx * xIdx; 00129 B.coeffRef (1) -= xy * xIdx; 00130 B.coeffRef (2) -= xz * xIdx; 00131 B.coeffRef (3) -= point.x * static_cast<double>(xIdx); 00132 00133 B.coeffRef (5) -= yy * xIdx; 00134 B.coeffRef (6) -= yz * xIdx; 00135 B.coeffRef (7) -= point.y * static_cast<double>(xIdx); 00136 00137 B.coeffRef (10) -= zz * xIdx; 00138 B.coeffRef (11) -= point.z * static_cast<double>(xIdx); 00139 00140 B.coeffRef (15) -= xIdx; 00141 00142 C.coeffRef (0) -= xx * yIdx; 00143 C.coeffRef (1) -= xy * yIdx; 00144 C.coeffRef (2) -= xz * yIdx; 00145 C.coeffRef (3) -= point.x * static_cast<double>(yIdx); 00146 00147 C.coeffRef (5) -= yy * yIdx; 00148 C.coeffRef (6) -= yz * yIdx; 00149 C.coeffRef (7) -= point.y * static_cast<double>(yIdx); 00150 00151 C.coeffRef (10) -= zz * yIdx; 00152 C.coeffRef (11) -= point.z * static_cast<double>(yIdx); 00153 00154 C.coeffRef (15) -= yIdx; 00155 00156 D.coeffRef (0) += xx * xx_yy; 00157 D.coeffRef (1) += xy * xx_yy; 00158 D.coeffRef (2) += xz * xx_yy; 00159 D.coeffRef (3) += point.x * xx_yy; 00160 00161 D.coeffRef (5) += yy * xx_yy; 00162 D.coeffRef (6) += yz * xx_yy; 00163 D.coeffRef (7) += point.y * xx_yy; 00164 00165 D.coeffRef (10) += zz * xx_yy; 00166 D.coeffRef (11) += point.z * xx_yy; 00167 00168 D.coeffRef (15) += xx_yy; 00169 } 00170 00171 ++pointIt; 00172 } // while 00173 00174 pcl::common::internal::makeSymmetric (A); 00175 pcl::common::internal::makeSymmetric (B); 00176 pcl::common::internal::makeSymmetric (C); 00177 pcl::common::internal::makeSymmetric (D); 00178 00179 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero (); 00180 X.topLeftCorner<4,4> ().matrix () = A; 00181 X.block<4,4> (0, 8).matrix () = B; 00182 X.block<4,4> (8, 0).matrix () = B; 00183 X.block<4,4> (4, 4).matrix () = A; 00184 X.block<4,4> (4, 8).matrix () = C; 00185 X.block<4,4> (8, 4).matrix () = C; 00186 X.block<4,4> (8, 8).matrix () = D; 00187 00188 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm (X); 00189 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors (); 00190 00191 // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device. 00192 Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0); 00193 00194 double residual = residual_sqr.coeff (0); 00195 00196 projection_matrix.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0)); 00197 projection_matrix.coeffRef (1) = static_cast <float> (eigen_vectors.coeff (12)); 00198 projection_matrix.coeffRef (2) = static_cast <float> (eigen_vectors.coeff (24)); 00199 projection_matrix.coeffRef (3) = static_cast <float> (eigen_vectors.coeff (36)); 00200 projection_matrix.coeffRef (4) = static_cast <float> (eigen_vectors.coeff (48)); 00201 projection_matrix.coeffRef (5) = static_cast <float> (eigen_vectors.coeff (60)); 00202 projection_matrix.coeffRef (6) = static_cast <float> (eigen_vectors.coeff (72)); 00203 projection_matrix.coeffRef (7) = static_cast <float> (eigen_vectors.coeff (84)); 00204 projection_matrix.coeffRef (8) = static_cast <float> (eigen_vectors.coeff (96)); 00205 projection_matrix.coeffRef (9) = static_cast <float> (eigen_vectors.coeff (108)); 00206 projection_matrix.coeffRef (10) = static_cast <float> (eigen_vectors.coeff (120)); 00207 projection_matrix.coeffRef (11) = static_cast <float> (eigen_vectors.coeff (132)); 00208 00209 if (projection_matrix.coeff (0) < 0) 00210 projection_matrix *= -1.0; 00211 00212 return (residual); 00213 } 00214 00215 #endif