00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
00037 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
00038
00039
00041 template <typename PointSource, typename PointTarget> inline void
00042 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
00043 const pcl::PointCloud<PointSource> &cloud_src,
00044 const pcl::PointCloud<PointTarget> &cloud_tgt,
00045 Eigen::Matrix4f &transformation_matrix)
00046 {
00047
00048 transformation_matrix.setIdentity ();
00049
00050 Eigen::Vector4f centroid_src, centroid_tgt;
00051
00052 compute3DCentroid (cloud_src, centroid_src);
00053 compute3DCentroid (cloud_tgt, centroid_tgt);
00054
00055
00056 Eigen::MatrixXf cloud_src_demean;
00057 demeanPointCloud (cloud_src, centroid_src, cloud_src_demean);
00058
00059 Eigen::MatrixXf cloud_tgt_demean;
00060 demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
00061
00062
00063 Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00064
00065
00066 Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00067 Eigen::Matrix3f u = svd.matrixU ();
00068 Eigen::Matrix3f v = svd.matrixV ();
00069
00070
00071 if (u.determinant () * v.determinant () < 0)
00072 {
00073 for (int x = 0; x < 3; ++x)
00074 v (x, 2) *= -1;
00075 }
00076
00077 Eigen::Matrix3f R = v * u.transpose ();
00078
00079
00080 transformation_matrix.topLeftCorner<3, 3> () = R;
00081 Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00082 transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00083
00084 }
00085
00087 template <typename PointSource, typename PointTarget> void
00088 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
00089 const pcl::PointCloud<PointSource> &cloud_src,
00090 const std::vector<int> &indices_src,
00091 const pcl::PointCloud<PointTarget> &cloud_tgt,
00092 Eigen::Matrix4f &transformation_matrix)
00093 {
00094 if (indices_src.size () != cloud_tgt.points.size ())
00095 {
00096 ROS_ERROR ("[pcl::estimateRigidTransformationSVD] Number or points in source (%zu) differs than target (%zu)!", indices_src.size (), cloud_tgt.points.size ());
00097 return;
00098 }
00099
00100
00101 transformation_matrix.setIdentity ();
00102
00103 Eigen::Vector4f centroid_src, centroid_tgt;
00104
00105 compute3DCentroid (cloud_src, indices_src, centroid_src);
00106 compute3DCentroid (cloud_tgt, centroid_tgt);
00107
00108
00109 Eigen::MatrixXf cloud_src_demean;
00110 demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00111
00112 Eigen::MatrixXf cloud_tgt_demean;
00113 demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
00114
00115
00116 Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00117
00118
00119 Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00120 Eigen::Matrix3f u = svd.matrixU ();
00121 Eigen::Matrix3f v = svd.matrixV ();
00122
00123
00124 if (u.determinant () * v.determinant () < 0)
00125 {
00126 for (int x = 0; x < 3; ++x)
00127 v (x, 2) *= -1;
00128 }
00129
00130 Eigen::Matrix3f R = v * u.transpose ();
00131
00132
00133 transformation_matrix.topLeftCorner<3, 3> () = R;
00134 Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00135 transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00136 }
00137
00138
00140 template <typename PointSource, typename PointTarget> inline void
00141 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
00142 const pcl::PointCloud<PointSource> &cloud_src,
00143 const std::vector<int> &indices_src,
00144 const pcl::PointCloud<PointTarget> &cloud_tgt,
00145 const std::vector<int> &indices_tgt,
00146 Eigen::Matrix4f &transformation_matrix)
00147 {
00148 if (indices_src.size () != indices_tgt.size ())
00149 {
00150 ROS_ERROR ("[pcl::estimateRigidTransformationSVD] Number or points in source (%zu) differs than target (%zu)!", indices_src.size (), indices_tgt.size ());
00151 return;
00152 }
00153
00154
00155 transformation_matrix.setIdentity ();
00156
00157 Eigen::Vector4f centroid_src, centroid_tgt;
00158
00159 compute3DCentroid (cloud_src, indices_src, centroid_src);
00160 compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
00161
00162
00163 Eigen::MatrixXf cloud_src_demean;
00164 demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00165
00166 Eigen::MatrixXf cloud_tgt_demean;
00167 demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
00168
00169
00170 Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00171
00172
00173 Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00174 Eigen::Matrix3f u = svd.matrixU ();
00175 Eigen::Matrix3f v = svd.matrixV ();
00176
00177
00178 if (u.determinant () * v.determinant () < 0)
00179 {
00180 for (int x = 0; x < 3; ++x)
00181 v (x, 2) *= -1;
00182 }
00183
00184 Eigen::Matrix3f R = v * u.transpose ();
00185
00186
00187 transformation_matrix.topLeftCorner<3, 3> () = R;
00188 Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00189 transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00190 }
00191
00193 template <typename PointSource, typename PointTarget> inline void
00194 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
00195 const pcl::PointCloud<PointSource> &cloud_src,
00196 const pcl::PointCloud<PointTarget> &cloud_tgt,
00197 const std::vector<pcl::registration::Correspondence> &correspondences,
00198 Eigen::Matrix4f &transformation_matrix)
00199 {
00200 transformation_matrix.setIdentity ();
00201
00202 std::vector<int> indices_src, indices_tgt;
00203 pcl::registration::getQueryIndices(correspondences, indices_src);
00204 pcl::registration::getMatchIndices(correspondences, indices_tgt);
00205
00206
00207 Eigen::Vector4f centroid_src, centroid_tgt;
00208
00209 compute3DCentroid (cloud_src, indices_src, centroid_src);
00210 compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
00211
00212
00213 Eigen::MatrixXf cloud_src_demean;
00214 demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00215
00216 Eigen::MatrixXf cloud_tgt_demean;
00217 demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
00218
00219
00220 Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00221
00222
00223 Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00224 Eigen::Matrix3f u = svd.matrixU ();
00225 Eigen::Matrix3f v = svd.matrixV ();
00226
00227
00228 if (u.determinant () * v.determinant () < 0)
00229 {
00230 for (int x = 0; x < 3; ++x)
00231 v (x, 2) *= -1;
00232 }
00233
00234 Eigen::Matrix3f R = v * u.transpose ();
00235
00236
00237 transformation_matrix.topLeftCorner<3, 3> () = R;
00238 Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00239 transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00240 }
00241
00242 #endif