rigid_transforms.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  */
00036 
00037 #ifndef PCL_COMMON_IMPL_RIGID_TRANSFORMS_H_
00038 #define PCL_COMMON_IMPL_RIGID_TRANSFORMS_H_
00039 
00040 #include "pcl/common/centroid.h"
00041 #include <Eigen/SVD>
00042 
00044 template <typename PointSource, typename PointTarget> void
00045 pcl::estimateRigidTransformationSVD (const pcl::PointCloud<PointSource> &cloud_src, 
00046                                      const pcl::PointCloud<PointTarget> &cloud_tgt, 
00047                                      Eigen::Matrix4f &transformation_matrix)
00048 {
00049   if (cloud_src.points.size () != cloud_tgt.points.size ())
00050   {
00051     PCL_ERROR ("[pcl::estimateRigidTransformationSVD] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)cloud_src.points.size (), (unsigned long)cloud_tgt.points.size ());
00052     return;
00053   }
00054 
00055   // <cloud_src,cloud_src> is the source dataset
00056   transformation_matrix.setIdentity ();
00057 
00058   Eigen::Vector4f centroid_src, centroid_tgt;
00059   // Estimate the centroids of source, target
00060   compute3DCentroid (cloud_src, centroid_src);
00061   compute3DCentroid (cloud_tgt, centroid_tgt);
00062 
00063   // Subtract the centroids from source, target
00064   Eigen::MatrixXf cloud_src_demean;
00065   demeanPointCloud (cloud_src, centroid_src, cloud_src_demean);
00066 
00067   Eigen::MatrixXf cloud_tgt_demean;
00068   demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
00069 
00070   // Assemble the correlation matrix H = source * target'
00071   Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00072 
00073   // Compute the Singular Value Decomposition
00074   Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00075   Eigen::Matrix3f u = svd.matrixU ();
00076   Eigen::Matrix3f v = svd.matrixV ();
00077 
00078   // Compute R = V * U'
00079   if (u.determinant () * v.determinant () < 0)
00080   {
00081     for (int x = 0; x < 3; ++x)
00082       v (x, 2) *= -1;
00083   }
00084 
00085   Eigen::Matrix3f R = v * u.transpose ();
00086 
00087   // Return the correct transformation
00088   transformation_matrix.topLeftCorner<3, 3> () = R;
00089   Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00090   transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00091 }
00092 
00094 template <typename PointSource, typename PointTarget> void
00095 pcl::estimateRigidTransformationSVD (const pcl::PointCloud<PointSource> &cloud_src, 
00096                                      const std::vector<int> &indices_src, 
00097                                      const pcl::PointCloud<PointTarget> &cloud_tgt, 
00098                                      const std::vector<int> &indices_tgt, 
00099                                      Eigen::Matrix4f &transformation_matrix)
00100 {
00101   if (indices_src.size () != indices_tgt.size ())
00102   {
00103     PCL_ERROR ("[pcl::estimateRigidTransformationSVD] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00104     return;
00105   }
00106 
00107   // <cloud_src,cloud_src> is the source dataset
00108   transformation_matrix.setIdentity ();
00109 
00110   Eigen::Vector4f centroid_src, centroid_tgt;
00111   // Estimate the centroids of source, target
00112   compute3DCentroid (cloud_src, indices_src, centroid_src);
00113   compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
00114 
00115   // Subtract the centroids from source, target
00116   Eigen::MatrixXf cloud_src_demean;
00117   demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00118 
00119   Eigen::MatrixXf cloud_tgt_demean;
00120   demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
00121 
00122   // Assemble the correlation matrix H = source * target'
00123   Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00124 
00125   // Compute the Singular Value Decomposition
00126   Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00127   Eigen::Matrix3f u = svd.matrixU ();
00128   Eigen::Matrix3f v = svd.matrixV ();
00129 
00130   // Compute R = V * U'
00131   if (u.determinant () * v.determinant () < 0)
00132   {
00133     for (int x = 0; x < 3; ++x)
00134       v (x, 2) *= -1;
00135   }
00136 
00137   Eigen::Matrix3f R = v * u.transpose ();
00138 
00139   // Return the correct transformation
00140   transformation_matrix.topLeftCorner<3, 3> () = R;
00141   Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00142   transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00143 }
00144 
00145 
00147 template <typename PointSource, typename PointTarget> void
00148 pcl::estimateRigidTransformationSVD (const pcl::PointCloud<PointSource> &cloud_src, 
00149                                      const std::vector<int> &indices_src, 
00150                                      const pcl::PointCloud<PointTarget> &cloud_tgt, 
00151                                      Eigen::Matrix4f &transformation_matrix)
00152 {
00153   if (indices_src.size () != cloud_tgt.points.size ())
00154   {
00155     PCL_ERROR ("[pcl::estimateRigidTransformationSVD] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)cloud_tgt.points.size ());
00156     return;
00157   }
00158 
00159   // <cloud_src,cloud_src> is the source dataset
00160   transformation_matrix.setIdentity ();
00161 
00162   Eigen::Vector4f centroid_src, centroid_tgt;
00163   // Estimate the centroids of source, target
00164   compute3DCentroid (cloud_src, indices_src, centroid_src);
00165   compute3DCentroid (cloud_tgt, centroid_tgt);
00166 
00167   // Subtract the centroids from source, target
00168   Eigen::MatrixXf cloud_src_demean;
00169   demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00170 
00171   Eigen::MatrixXf cloud_tgt_demean;
00172   demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
00173 
00174   // Assemble the correlation matrix H = source * target'
00175   Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00176 
00177   // Compute the Singular Value Decomposition
00178   Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00179   const Eigen::Matrix3f &u = svd.matrixU ();
00180   const Eigen::Matrix3f &v = svd.matrixV ();
00181 
00182   // Compute R = V * U'
00183   if (u.determinant () * v.determinant () < 0)
00184   {
00185     for (int x = 0; x < 3; ++x)
00186       v (x, 2) *= -1;
00187   }
00188 
00189   Eigen::Matrix3f R = v * u.transpose ();
00190 
00191   // Return the correct transformation
00192   transformation_matrix.topLeftCorner<3, 3> () = R;
00193   Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00194   transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00195 }
00196 
00197 #endif // PCL_COMMON_IMPL_RIGID_TRANSFORMS_H_