door_handle_detector: transforms.h 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$
00028  *
00029  */
00030 
00033 #ifndef _CLOUD_GEOMETRY_TRANSFORMS_H_
00034 #define _CLOUD_GEOMETRY_TRANSFORMS_H_
00035 
00036 // ROS includes
00037 #include <sensor_msgs/PointCloud.h>
00038 #include <geometry_msgs/Point32.h>
00039 
00040 #include <vector>
00041 #include <Eigen/Core>
00042 #include <Eigen/LU>
00043 
00044 namespace cloud_geometry
00045 {
00046   namespace transforms
00047   {
00048 
00062         bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a, const sensor_msgs::PointCloud& pc_b,
00063                                                                                   Eigen::Matrix4d &transformation);
00064 
00079         bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a, const std::vector<int>& indices_a,
00080                                                                                   const sensor_msgs::PointCloud& pc_b, const std::vector<int>& indices_b,
00081                                                                                   Eigen::Matrix4d &transformation);
00082 
00083     void getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const std::vector<double> &plane_b, float tx, float ty, float tz,
00084                                         Eigen::Matrix4d &transformation);
00085     void getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b, float tx, float ty, float tz,
00086                                         Eigen::Matrix4d &transformation);
00087 
00089 
00094     inline void
00095       transformPoint (const geometry_msgs::Point32 &point_in, geometry_msgs::Point32 &point_out, const Eigen::Matrix4d &transformation)
00096     {
00097       point_out.x = transformation (0, 0) * point_in.x + transformation (0, 1) * point_in.y + transformation (0, 2) * point_in.z + transformation (0, 3);
00098       point_out.y = transformation (1, 0) * point_in.x + transformation (1, 1) * point_in.y + transformation (1, 2) * point_in.z + transformation (1, 3);
00099       point_out.z = transformation (2, 0) * point_in.x + transformation (2, 1) * point_in.y + transformation (2, 2) * point_in.z + transformation (2, 3);
00100     }
00101 
00103 
00108     inline void
00109       transformPoints (const std::vector<geometry_msgs::Point32> &points_in, std::vector<geometry_msgs::Point32> &points_out, const Eigen::Matrix4d &transformation)
00110     {
00111       points_out.resize (points_in.size ());
00112       for (unsigned i = 0; i < points_in.size (); i++)
00113         transformPoint (points_in.at (i), points_out[i], transformation);
00114     }
00115 
00116 
00118 
00122     inline void
00123       getInverseTransformation (const Eigen::Matrix4d &transformation, Eigen::Matrix4d &transformation_inverse)
00124     {
00125       float tx = transformation (0, 3);
00126       float ty = transformation (1, 3);
00127       float tz = transformation (2, 3);
00128 
00129       transformation_inverse (0, 0) = transformation (0, 0);
00130       transformation_inverse (0, 1) = transformation (1, 0);
00131       transformation_inverse (0, 2) = transformation (2, 0);
00132       transformation_inverse (0, 3) = - (transformation (0, 0) * tx + transformation (0, 1) * ty + transformation (0, 2) * tz);
00133 
00134 
00135       transformation_inverse (1, 0) = transformation (0, 1);
00136       transformation_inverse (1, 1) = transformation (1, 1);
00137       transformation_inverse (1, 2) = transformation (2, 1);
00138       transformation_inverse (1, 3) = - (transformation (1, 0) * tx + transformation (1, 1) * ty + transformation (1, 2) * tz);
00139 
00140       transformation_inverse (2, 0) = transformation (0, 2);
00141       transformation_inverse (2, 1) = transformation (1, 2);
00142       transformation_inverse (2, 2) = transformation (2, 2);
00143       transformation_inverse (2, 3) = - (transformation (2, 0) * tx + transformation (2, 1) * ty + transformation (2, 2) * tz);
00144 
00145       transformation_inverse (3, 0) = 0;
00146       transformation_inverse (3, 1) = 0;
00147       transformation_inverse (3, 2) = 0;
00148       transformation_inverse (3, 3) = 1;
00149     }
00150 
00151     void convertAxisAngleToRotationMatrix (const geometry_msgs::Point32 &axis, double angle, Eigen::Matrix3d &rotation);
00152   }
00153 }
00154 
00155 #endif