door_handle_detector: transforms.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id$
00028  *
00029  */
00030 
00033 #include <door_handle_detector/geometry/angles.h>
00034 #include <door_handle_detector/geometry/transforms.h>
00035 #include <door_handle_detector/geometry/nearest.h>
00036 
00037 #include <Eigen/SVD>
00038 #include <Eigen/Eigenvalues>
00039 
00040 
00041 namespace cloud_geometry
00042 {
00043   namespace transforms
00044   {
00045 
00049   bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const sensor_msgs::PointCloud& pc_b_,
00050                   Eigen::Matrix4d &transformation)
00051   {
00052 
00053           assert (pc_a_.points.size()==pc_b_.points.size());
00054 
00055           sensor_msgs::PointCloud pc_a = pc_a_;
00056           sensor_msgs::PointCloud pc_b = pc_b_;
00057 
00058           // translate both point cloud so that their centroid will be in origin
00059           geometry_msgs::Point32 centroid_a;
00060           geometry_msgs::Point32 centroid_b;
00061 
00062           nearest::computeCentroid(pc_a, centroid_a);
00063           nearest::computeCentroid(pc_b, centroid_b);
00064 
00065           for (size_t i=0;i<pc_a.points.size();++i) {
00066                   pc_a.points[i].x -= centroid_a.x;
00067                   pc_a.points[i].y -= centroid_a.y;
00068                   pc_a.points[i].z -= centroid_a.z;
00069           }
00070 
00071           for (size_t i=0;i<pc_b.points.size();++i) {
00072                   pc_b.points[i].x -= centroid_b.x;
00073                   pc_b.points[i].y -= centroid_b.y;
00074                   pc_b.points[i].z -= centroid_b.z;
00075           }
00076 
00077           // solve for rotation
00078           Eigen::Matrix3d correlation;
00079           correlation.setZero();
00080 
00081           for (size_t i=0;i<pc_a.points.size();++i) {
00082                   correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
00083                   correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
00084                   correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;
00085 
00086                   correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
00087                   correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
00088                   correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;
00089 
00090                   correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
00091                   correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
00092                   correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
00093           }
00094 
00095           Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation,Eigen::ComputeThinU|Eigen::ComputeThinV);
00096 
00097           Eigen::Matrix3d Ut = svd.matrixU().transpose();
00098           Eigen::Matrix3d V = svd.matrixV();
00099           Eigen::Matrix3d X = V*Ut;
00100 
00101           double det = X.determinant();
00102           if (det<0) {
00103                   V.col(2) = -V.col(2);
00104                   X = V*Ut;
00105           }
00106 
00107           transformation.setZero();
00108           transformation.topLeftCorner(3,3) = X;
00109           transformation(3,3) = 1;
00110 
00111           sensor_msgs::PointCloud pc_rotated_a;
00112           transformPoints(pc_a_.points, pc_rotated_a.points, transformation);
00113 
00114           geometry_msgs::Point32 centroid_rotated_a;
00115           nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);
00116 
00117           transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
00118           transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
00119           transformation(2,3) = centroid_b.z - centroid_rotated_a.z;
00120 
00121           return true;
00122 
00123   }
00124 
00125 
00129         bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const std::vector<int>& indices_a,
00130                                                                                   const sensor_msgs::PointCloud& pc_b_, const std::vector<int>& indices_b,
00131                                                                                   Eigen::Matrix4d &transformation)
00132         {
00133 
00134           assert (indices_a.size()==indices_b.size());
00135 
00136           sensor_msgs::PointCloud pc_a;
00137           sensor_msgs::PointCloud pc_b;
00138 
00139           getPointCloud(pc_a_, indices_a, pc_a);
00140           getPointCloud(pc_b_, indices_b, pc_b);
00141 
00142           // translate both point cloud so that their centroid will be in origin
00143           geometry_msgs::Point32 centroid_a;
00144           geometry_msgs::Point32 centroid_b;
00145 
00146           nearest::computeCentroid(pc_a, centroid_a);
00147           nearest::computeCentroid(pc_b, centroid_b);
00148 
00149           for (size_t i=0;i<pc_a.points.size();++i) {
00150                   pc_a.points[i].x -= centroid_a.x;
00151                   pc_a.points[i].y -= centroid_a.y;
00152                   pc_a.points[i].z -= centroid_a.z;
00153           }
00154 
00155           for (size_t i=0;i<pc_b.points.size();++i) {
00156                   pc_b.points[i].x -= centroid_b.x;
00157                   pc_b.points[i].y -= centroid_b.y;
00158                   pc_b.points[i].z -= centroid_b.z;
00159           }
00160 
00161           // solve for rotation
00162           Eigen::Matrix3d correlation;
00163           correlation.setZero();
00164 
00165           for (size_t i=0;i<pc_a.points.size();++i) {
00166                   correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
00167                   correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
00168                   correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;
00169 
00170                   correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
00171                   correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
00172                   correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;
00173 
00174                   correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
00175                   correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
00176                   correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
00177           }
00178 
00179           Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation,Eigen::ComputeThinU|Eigen::ComputeThinV);
00180 
00181           Eigen::Matrix3d Ut = svd.matrixU().transpose();
00182           Eigen::Matrix3d V = svd.matrixV();
00183           Eigen::Matrix3d X = V*Ut;
00184 
00185           double det = X.determinant();
00186           if (det<0) {
00187                   V.col(2) = -V.col(2);
00188                   X = V*Ut;
00189           }
00190 
00191           transformation.setZero();
00192           transformation.topLeftCorner(3,3) = X;
00193           transformation(3,3) = 1;
00194 
00195           sensor_msgs::PointCloud pc_rotated_a;
00196           getPointCloud(pc_a_, indices_a, pc_a);
00197           transformPoints(pc_a.points, pc_rotated_a.points, transformation);
00198 
00199           geometry_msgs::Point32 centroid_rotated_a;
00200           nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);
00201 
00202           transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
00203           transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
00204           transformation(2,3) = centroid_b.z - centroid_rotated_a.z;
00205 
00206 
00207 //        transformation.setIdentity();
00208 //        transformation(0,3) = centroid_b.x - centroid_a.x;
00209 //        transformation(1,3) = centroid_b.y - centroid_a.y;
00210 //        transformation(2,3) = centroid_b.z - centroid_a.z;
00211 
00212           return true;
00213 
00214   }
00215 
00217 
00225     void
00226       getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const std::vector<double> &plane_b,
00227                                      float tx, float ty, float tz, Eigen::Matrix4d &transformation)
00228     {
00229       double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
00230       // Compute the rotation axis R = Nplane x (0, 0, 1)
00231       geometry_msgs::Point32 r_axis;
00232       r_axis.x = plane_a[1]*plane_b[2] - plane_a[2]*plane_b[1];
00233       r_axis.y = plane_a[2]*plane_b[0] - plane_a[0]*plane_b[2];
00234       r_axis.z = plane_a[0]*plane_b[1] - plane_a[1]*plane_b[0];
00235 
00236       if (r_axis.z < 0)
00237         angle = -angle;
00238 
00239       // Build a normalized quaternion
00240       double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
00241       double x = r_axis.x * s;
00242       double y = r_axis.y * s;
00243       double z = r_axis.z * s;
00244       double w = cos (0.5 * angle);
00245 
00246       // Convert the quaternion to a 3x3 matrix
00247       double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
00248       double wx = w * x; double wy = w * y; double wz = w * z;
00249       double xy = x * y; double xz = x * z; double yz = y * z;
00250 
00251       transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
00252       transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
00253       transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
00254       transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
00255     }
00256 
00258 
00266     void
00267       getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b,
00268                                      float tx, float ty, float tz, Eigen::Matrix4d &transformation)
00269     {
00270       double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
00271       // Compute the rotation axis R = Nplane x (0, 0, 1)
00272       geometry_msgs::Point32 r_axis;
00273       r_axis.x = plane_a[1]*plane_b.z - plane_a[2]*plane_b.y;
00274       r_axis.y = plane_a[2]*plane_b.x - plane_a[0]*plane_b.z;
00275       r_axis.z = plane_a[0]*plane_b.y - plane_a[1]*plane_b.x;
00276 
00277       if (r_axis.z < 0)
00278         angle = -angle;
00279 
00280       // Build a normalized quaternion
00281       double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
00282       double x = r_axis.x * s;
00283       double y = r_axis.y * s;
00284       double z = r_axis.z * s;
00285       double w = cos (0.5 * angle);
00286 
00287       // Convert the quaternion to a 3x3 matrix
00288       double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
00289       double wx = w * x; double wy = w * y; double wz = w * z;
00290       double xy = x * y; double xz = x * z; double yz = y * z;
00291 
00292       transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
00293       transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
00294       transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
00295       transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
00296     }
00297 
00299 
00306     void
00307       convertAxisAngleToRotationMatrix (const geometry_msgs::Point32 &axis, double angle, Eigen::Matrix3d &rotation)
00308     {
00309       double cos_a = cos (angle);
00310       double sin_a = sin (angle);
00311       double cos_a_m = 1.0 - cos_a;
00312 
00313       double a_xy = axis.x * axis.y * cos_a_m;
00314       double a_xz = axis.x * axis.z * cos_a_m;
00315       double a_yz = axis.y * axis.z * cos_a_m;
00316 
00317       double s_x = sin_a * axis.x;
00318       double s_y = sin_a * axis.y;
00319       double s_z = sin_a * axis.z;
00320 
00321       rotation (0, 0) = cos_a + axis.x * axis.x * cos_a_m;
00322       rotation (0, 1) = a_xy - s_z;
00323       rotation (0, 2) = a_xz + s_y;
00324       rotation (1, 0) = a_xy + s_z;
00325       rotation (1, 1) = cos_a + axis.y * axis.y * cos_a_m;
00326       rotation (1, 2) = a_yz - s_x;
00327       rotation (2, 0) = a_xz - s_y;
00328       rotation (2, 1) = a_yz + s_x;
00329       rotation (2, 2) = cos_a + axis.z * axis.z * cos_a_m;
00330     }
00331 
00332   }
00333 }