transform.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008 Dejan Pangercic <pangercic -=- 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: transform.h 21045 2009-08-07 20:52:44Z dejanpan $
00028  *
00029  */
00030 
00033 #ifndef _PLAYER_LOG_ACTARRAY_TRANSFORM_H_
00034 #define _PLAYER_LOG_ACTARRAY_TRANSFORM_H_
00035 
00036 // ROS includes
00037 #include <geometry_msgs/Point32.h>
00038 #include <sensor_msgs/PointCloud.h>
00039 #include <kdl/frames.hpp>
00040 
00041 using namespace KDL;
00042 using namespace sensor_msgs;
00043 using namespace geometry_msgs;
00044 
00045 namespace player_log_actarray
00046 {
00047   
00048   namespace transform
00049   {
00051     // rotate PointCloud
00053     bool rotatePointCloud (const PointCloud &cloud_in, PointCloud &cloud_out, double angle, Point32 axis)
00054     { 
00055       Vector p,p2; 
00056       Rotation R;
00057         if(axis.x)
00058             R.DoRotX(angle);
00059           else if (axis.y)
00060             R.DoRotY(angle);
00061           else if  (axis.z)
00062             R.DoRotZ(angle);
00063           else
00064             {
00065               ROS_ERROR("Axis must [1 0 0] format!");
00066               return false;
00067             }
00068         cloud_out.points.resize(0);
00069         cloud_out.header.frame_id =  cloud_in.header.frame_id;
00070         cloud_out.channels.resize (cloud_in.channels.size());
00071       for (int i = 0; i < cloud_in.channels.size(); i++)
00072         cloud_out.channels[i].name =  cloud_in.channels[i].name;
00073 
00074       ROS_INFO("in rpc %f %f %f %f", angle, axis.x, axis.y, axis.z);
00075       for (unsigned int point = 0; point < cloud_in.points.size(); point++)
00076         {
00077           //resize
00078           for (unsigned int d = 0; d < cloud_in.channels.size (); d++)
00079             cloud_out.channels[d].values.resize (point + 1);
00080           //rotate, fill in
00081           p.x(cloud_in.points[point].x);
00082           p.y(cloud_in.points[point].y);
00083           p.z(cloud_in.points[point].z);
00084           p2 = R*p;
00085           cloud_out.points.resize(point + 1);
00086           cloud_out.points[point].x = p2.x();
00087           cloud_out.points[point].y = p2.y();
00088           cloud_out.points[point].z = p2.z();
00089 
00090           // Save the rest of the values
00091           for (int i = 0; i < cloud_in.channels.size(); i++)
00092             {
00093               cloud_out.channels[i].values[point] =  cloud_in.channels[i].values[point];
00094             }
00095         }
00096       return true;
00097     }
00098 
00100     // shift PointCloud
00102     bool translatePointCloud (const PointCloud &cloud_in, PointCloud &cloud_out, Point32 trans)
00103     {
00104       cloud_out.points.resize(0);
00105       cloud_out.header.frame_id =  cloud_in.header.frame_id;
00106       cloud_out.channels.resize (cloud_in.channels.size());
00107       for (int i = 0; i < cloud_in.channels.size(); i++)
00108         cloud_out.channels[i].name =  cloud_in.channels[i].name;
00109 
00110        for (unsigned int point = 0; point < cloud_in.points.size(); point++)
00111         {
00112           //resize
00113           for (unsigned int d = 0; d < cloud_in.channels.size (); d++)
00114             cloud_out.channels[d].values.resize (point + 1);
00115           //rotate, fill in
00116           cloud_out.points.resize(point + 1);
00117           cloud_out.points[point].x = cloud_in.points[point].x + trans.x;
00118           cloud_out.points[point].y = cloud_in.points[point].y + trans.y;
00119           cloud_out.points[point].z = cloud_in.points[point].z + trans.z;
00120           // Save the rest of the values
00121           for (int i = 0; i < cloud_in.channels.size(); i++)
00122             cloud_out.channels[i].values[point] =  cloud_in.channels[i].values[point];
00123         }
00124        return true;
00125     } 
00126   }
00127 }
00128 #endif


player_log_actarray
Author(s): Radu Bogdan Rusu
autogenerated on Mon Oct 6 2014 09:38:35