stereo_wall_detection: transforms.cpp Source 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: transforms.cpp 23275 2009-08-28 20:27:09Z mariusmuja $
00028  *
00029  */
00030 
00033 #include <stereo_wall_detection/geometry/angles.h>
00034 #include <stereo_wall_detection/geometry/transforms.h>
00035 #include <stereo_wall_detection/geometry/nearest.h>
00036 
00037 #include <Eigen/SVD>
00038 
00039 
00040 namespace cloud_geometry
00041 {
00042   namespace transforms
00043   {
00044 
00048   bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const sensor_msgs::PointCloud& pc_b_,
00049                   Eigen::Matrix4d &transformation)
00050   {
00051 
00052           assert (pc_a_.points.size()==pc_b_.points.size());
00053 
00054           sensor_msgs::PointCloud pc_a = pc_a_;
00055           sensor_msgs::PointCloud pc_b = pc_b_;
00056 
00057           // translate both point cloud so that their centroid will be in origin
00058           geometry_msgs::Point32 centroid_a;
00059           geometry_msgs::Point32 centroid_b;
00060 
00061           nearest::computeCentroid(pc_a, centroid_a);
00062           nearest::computeCentroid(pc_b, centroid_b);
00063 
00064           for (size_t i=0;i<pc_a.points.size();++i) {
00065                   pc_a.points[i].x -= centroid_a.x;
00066                   pc_a.points[i].y -= centroid_a.y;
00067                   pc_a.points[i].z -= centroid_a.z;
00068           }
00069 
00070           for (size_t i=0;i<pc_b.points.size();++i) {
00071                   pc_b.points[i].x -= centroid_b.x;
00072                   pc_b.points[i].y -= centroid_b.y;
00073                   pc_b.points[i].z -= centroid_b.z;
00074           }
00075 
00076           // solve for rotation
00077           Eigen::Matrix3d correlation;
00078           correlation.setZero();
00079 
00080           for (size_t i=0;i<pc_a.points.size();++i) {
00081                   correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
00082                   correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
00083                   correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;
00084 
00085                   correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
00086                   correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
00087                   correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;
00088 
00089                   correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
00090                   correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
00091                   correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
00092           }
00093 
00094           Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);
00095 
00096           Eigen::Matrix3d Ut = svd.matrixU().transpose();
00097           Eigen::Matrix3d V = svd.matrixV();
00098           Eigen::Matrix3d X = V*Ut;
00099 
00100           double det = X.determinant();
00101           if (det<0) {
00102                   V.col(2) = -V.col(2);
00103                   X = V*Ut;
00104           }
00105 
00106           transformation.setZero();
00107           transformation.topLeftCorner<3,3>() = X;
00108           transformation(3,3) = 1;
00109 
00110           sensor_msgs::PointCloud pc_rotated_a;
00111           transformPoints(pc_a_.points, pc_rotated_a.points, transformation);
00112 
00113           geometry_msgs::Point32 centroid_rotated_a;
00114           nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);
00115 
00116           transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
00117           transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
00118           transformation(2,3) = centroid_b.z - centroid_rotated_a.z;
00119 
00120           return true;
00121 
00122   }
00123 
00124 
00128         bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const std::vector<int>& indices_a,
00129                                                                                   const sensor_msgs::PointCloud& pc_b_, const std::vector<int>& indices_b,
00130                                                                                   Eigen::Matrix4d &transformation)
00131         {
00132 
00133           assert (indices_a.size()==indices_b.size());
00134 
00135           sensor_msgs::PointCloud pc_a;
00136           sensor_msgs::PointCloud pc_b;
00137 
00138           getPointCloud(pc_a_, indices_a, pc_a);
00139           getPointCloud(pc_b_, indices_b, pc_b);
00140 
00141           // translate both point cloud so that their centroid will be in origin
00142           geometry_msgs::Point32 centroid_a;
00143           geometry_msgs::Point32 centroid_b;
00144 
00145           nearest::computeCentroid(pc_a, centroid_a);
00146           nearest::computeCentroid(pc_b, centroid_b);
00147 
00148           for (size_t i=0;i<pc_a.points.size();++i) {
00149                   pc_a.points[i].x -= centroid_a.x;
00150                   pc_a.points[i].y -= centroid_a.y;
00151                   pc_a.points[i].z -= centroid_a.z;
00152           }
00153 
00154           for (size_t i=0;i<pc_b.points.size();++i) {
00155                   pc_b.points[i].x -= centroid_b.x;
00156                   pc_b.points[i].y -= centroid_b.y;
00157                   pc_b.points[i].z -= centroid_b.z;
00158           }
00159 
00160           // solve for rotation
00161           Eigen::Matrix3d correlation;
00162           correlation.setZero();
00163 
00164           for (size_t i=0;i<pc_a.points.size();++i) {
00165                   correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
00166                   correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
00167                   correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;
00168 
00169                   correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
00170                   correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
00171                   correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;
00172 
00173                   correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
00174                   correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
00175                   correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
00176           }
00177 
00178           Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);
00179 
00180           Eigen::Matrix3d Ut = svd.matrixU().transpose();
00181           Eigen::Matrix3d V = svd.matrixV();
00182           Eigen::Matrix3d X = V*Ut;
00183 
00184           double det = X.determinant();
00185           if (det<0) {
00186                   V.col(2) = -V.col(2);
00187                   X = V*Ut;
00188           }
00189 
00190           transformation.setZero();
00191           transformation.topLeftCorner<3,3>() = X;
00192           transformation(3,3) = 1;
00193 
00194           sensor_msgs::PointCloud pc_rotated_a;
00195           getPointCloud(pc_a_, indices_a, pc_a);
00196           transformPoints(pc_a.points, pc_rotated_a.points, transformation);
00197 
00198           geometry_msgs::Point32 centroid_rotated_a;
00199           nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);
00200 
00201           transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
00202           transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
00203           transformation(2,3) = centroid_b.z - centroid_rotated_a.z;
00204 
00205 
00206 //        transformation.setIdentity();
00207 //        transformation(0,3) = centroid_b.x - centroid_a.x;
00208 //        transformation(1,3) = centroid_b.y - centroid_a.y;
00209 //        transformation(2,3) = centroid_b.z - centroid_a.z;
00210 
00211           return true;
00212 
00213   }
00214 
00216 
00224     void
00225       getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const std::vector<double> &plane_b,
00226                                      float tx, float ty, float tz, Eigen::Matrix4d &transformation)
00227     {
00228       double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
00229       // Compute the rotation axis R = Nplane x (0, 0, 1)
00230       geometry_msgs::Point32 r_axis;
00231       r_axis.x = plane_a[1]*plane_b[2] - plane_a[2]*plane_b[1];
00232       r_axis.y = plane_a[2]*plane_b[0] - plane_a[0]*plane_b[2];
00233       r_axis.z = plane_a[0]*plane_b[1] - plane_a[1]*plane_b[0];
00234 
00235       if (r_axis.z < 0)
00236         angle = -angle;
00237 
00238       // Build a normalized quaternion
00239       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);
00240       double x = r_axis.x * s;
00241       double y = r_axis.y * s;
00242       double z = r_axis.z * s;
00243       double w = cos (0.5 * angle);
00244 
00245       // Convert the quaternion to a 3x3 matrix
00246       double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
00247       double wx = w * x; double wy = w * y; double wz = w * z;
00248       double xy = x * y; double xz = x * z; double yz = y * z;
00249 
00250       transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
00251       transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
00252       transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
00253       transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
00254     }
00255 
00257 
00265     void
00266       getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b,
00267                                      float tx, float ty, float tz, Eigen::Matrix4d &transformation)
00268     {
00269       double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
00270       // Compute the rotation axis R = Nplane x (0, 0, 1)
00271       geometry_msgs::Point32 r_axis;
00272       r_axis.x = plane_a[1]*plane_b.z - plane_a[2]*plane_b.y;
00273       r_axis.y = plane_a[2]*plane_b.x - plane_a[0]*plane_b.z;
00274       r_axis.z = plane_a[0]*plane_b.y - plane_a[1]*plane_b.x;
00275 
00276       if (r_axis.z < 0)
00277         angle = -angle;
00278 
00279       // Build a normalized quaternion
00280       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);
00281       double x = r_axis.x * s;
00282       double y = r_axis.y * s;
00283       double z = r_axis.z * s;
00284       double w = cos (0.5 * angle);
00285 
00286       // Convert the quaternion to a 3x3 matrix
00287       double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
00288       double wx = w * x; double wy = w * y; double wz = w * z;
00289       double xy = x * y; double xz = x * z; double yz = y * z;
00290 
00291       transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
00292       transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
00293       transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
00294       transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
00295     }
00296 
00298 
00305     void
00306       convertAxisAngleToRotationMatrix (const geometry_msgs::Point32 &axis, double angle, Eigen::Matrix3d &rotation)
00307     {
00308       double cos_a = cos (angle);
00309       double sin_a = sin (angle);
00310       double cos_a_m = 1.0 - cos_a;
00311 
00312       double a_xy = axis.x * axis.y * cos_a_m;
00313       double a_xz = axis.x * axis.z * cos_a_m;
00314       double a_yz = axis.y * axis.z * cos_a_m;
00315 
00316       double s_x = sin_a * axis.x;
00317       double s_y = sin_a * axis.y;
00318       double s_z = sin_a * axis.z;
00319 
00320       rotation (0, 0) = cos_a + axis.x * axis.x * cos_a_m;
00321       rotation (0, 1) = a_xy - s_z;
00322       rotation (0, 2) = a_xz + s_y;
00323       rotation (1, 0) = a_xy + s_z;
00324       rotation (1, 1) = cos_a + axis.y * axis.y * cos_a_m;
00325       rotation (1, 2) = a_yz - s_x;
00326       rotation (2, 0) = a_xz - s_y;
00327       rotation (2, 1) = a_yz + s_x;
00328       rotation (2, 2) = cos_a + axis.z * axis.z * cos_a_m;
00329     }
00330 
00331   }
00332 }