project_segments.cpp
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  * $Id: segmentation_cylinder.cpp 35522 2011-01-26 08:17:01Z rusu $
00035  *
00036  */
00037 
00038 
00039 
00040 
00041 
00042 #include <pcl/io/pcd_io.h>
00043 #include <pcl/point_types.h>
00044 #include <tf/tf.h>
00045 #include <pcl_ros/transforms.h>
00046 
00047 typedef pcl::PointXYZ PointT;
00048 
00049 int
00050   main (int argc, char** argv)
00051 {
00052   // All the objects needed
00053   pcl::PCDReader reader;
00054 
00055   pcl::PCDWriter writer;
00056 
00057   // Datasets
00058   pcl::PointCloud<PointT>::Ptr input_cloud (new pcl::PointCloud<PointT> ());
00059   pcl::PointCloud<PointT>::Ptr output_cloud (new pcl::PointCloud<PointT> ());
00060 
00061   // Read in the cloud data
00062   reader.read (std::string(argv[1]), *input_cloud);
00063   ROS_INFO ("PointCloud has: %zu data points.", input_cloud->points.size ());
00064   //compute the transform usig btTransform
00065   std::vector<btTransform> poses_first (100);
00066   std::vector<btTransform> poses_last (100);
00067   btTransform bt1,bt2,bt1_inverse,bt2_inverse,bt_relative;
00068   Eigen::Matrix4f transform_matrix;
00069   poses_first[0]=btTransform(btQuaternion(0.998846824056,0.0321420649378,-0.0347925244179,-0.00783517578594) ,btVector3(0.794980231638,2.61825838228,0.793744988276));
00070   poses_last[0]=btTransform(btQuaternion(0.998752133384,0.0207771171007,-0.0400542187211,-0.0214043693847) ,btVector3(0.196217134071,2.58788842192,0.802082517919));
00071   poses_first[11]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.854595096684,2.1117762422,0.83581194444));
00072   poses_last[11]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.304168821189,2.0881359323,0.835223245908));
00073   poses_first[12]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.87515422062,2.1264438802,0.688548777959));
00074   poses_last[12]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.226732063269,2.0704584748,0.677043344612));
00075   poses_first[20]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.794980231638,1.7,0.783785592596));
00076   poses_last[20]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.196217134071,1.67243435547,0.794664770984));
00077   poses_first[30]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.708097227301,1.12431635089,0.783785592596));
00078   poses_last[30]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.15762123548,1.09807397523,0.783569963996));
00079   poses_first[31]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.707196010134,1.15281266082,0.64182772675));
00080   poses_last[31]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.108505198317,1.11737966783,0.631973685219));
00081   poses_first[32]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.720480791782,1.14081120967,0.349637179347));
00082   poses_last[32]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.169155951244,1.09290312037,0.352896361082));
00083   poses_first[40]=btTransform(btQuaternion(0.69988310609,0.0125240341376,0.0516863610901,0.712274740852) ,btVector3(0.759088815798,0.447555838799,0.788543972139));
00084   poses_last[40]=btTransform(btQuaternion(0.679154223135,-0.182616350164,0.209965139294,0.679202068704) ,btVector3(0.328294951431,0.455333579402,0.536777352992));
00085   poses_first[50]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.811475455459,-0.159275054725,0.64285697828));
00086   poses_last[50]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.262167086028,-0.189935857791,0.635715051788));
00087   poses_first[60]=btTransform(btQuaternion(-0.0231417985797,-0.004722980116,0.0150256782545,0.999608113017) ,btVector3(0.970785923351,-0.51177511333,0.987374139302));
00088   poses_last[60]=btTransform(btQuaternion(-0.0757567991896,-0.0338534608379,0.714338624073,0.694863425951) ,btVector3(0.481417937933,-1.13998831455,0.975107082045));
00089   poses_first[61]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.985176471098,-0.75775319426,0.467585908667));
00090   poses_last[61]=btTransform(btQuaternion(1,0,0,0) ,btVector3(0.486508365334,-0.797871464651,0.460385745908));
00091   bt1=poses_first[std::atoi(argv[1])];
00092   bt2=poses_last[std::atoi(argv[1])];
00093   bt1_inverse=bt1.inverse();
00094   bt2_inverse=bt2.inverse();
00095   bt_relative=bt1_inverse * bt2;
00096   bt_relative=bt1 * bt2_inverse;
00097   ROS_INFO("The transform bt1 is %f, %f, %f ",bt1.getOrigin().x(),bt1.getOrigin().y(),bt1.getOrigin().z());
00098   ROS_INFO("The transform bt2 is%f, %f, %f ",bt2.getOrigin().x(),bt2.getOrigin().y(),bt2.getOrigin().z());
00099   ROS_INFO("The transform is %f, %f, %f ",bt_relative.getOrigin().x(),bt_relative.getOrigin().y(),bt_relative.getOrigin().z());
00100   ROS_INFO("The orientation is %f, %f, %f, %f",bt_relative.getRotation().x(),bt_relative.getRotation().y(),bt_relative.getRotation().z(),bt_relative.getRotation().w());
00101   //tf::StampedTransform c2h_transform (bt_relative);
00102   pcl_ros::transformAsMatrix (bt_relative, transform_matrix);
00103   pcl::transformPointCloud(*input_cloud, *output_cloud,transform_matrix);
00104   // Write the planar inliers to disk
00105   std::string s="data/transformed_" + std::string(argv[1]);
00106   ROS_INFO("String is: %s ",s.c_str());
00107   ROS_INFO ("PointCloud representing the planar component: %zu data points.", output_cloud->points.size ());
00108   writer.write (s.c_str(), *output_cloud, false);
00109   return (0);
00110 }
00111 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Thu May 23 2013 08:58:16