test_spin_image_local.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Hozefa Indorewala
00032  * Simple test file to test spin image estimation of a few points of a point cloud
00033  */
00034 
00035 #include <ros/ros.h>
00036 #include <pointcloud_registration/spin_image_estimation/spin_image_estimation.h>
00037 #include <pointcloud_registration/pointcloud_registration_point_types.h>
00038 #include <pcl/io/pcd_io.h>
00039 #include "pcl/kdtree/kdtree_flann.h" //for the kdtree
00040 #include <pcl/features/normal_3d_omp.h>
00041 
00042 #include "fstream" //for writing a ppm image file
00043 
00044 #define PCD_FILE "29_July_full_room_no_downsample.pcd"
00045 #define MAX_NN 300
00046 #define RADIUS 0.1
00047 #define SUB_DIVISIONS 10
00048 #define MAX_VALUE_PPM 20
00049 
00050 int main()
00051 {
00052   sensor_msgs::PointCloud2 cloud_blob;
00053   pcl::PointCloud<pcl::PointXYZ> cloud;
00054   pcl::SpinImageEstimation<pcl::PointXYZ, pcl::PointNormal, pcl::SpinImageLocal> s;
00055   pcl::PointCloud<pcl::SpinImageLocal> sil;
00056   std::vector<int> indices;
00057 
00058   int sil_indices_array[] = {277816, 205251, 198723, 221146, 271903};
00059   std::vector<int> sil_indices (sil_indices_array, sil_indices_array + sizeof(sil_indices_array) / sizeof(int) );
00060   if (pcl::io::loadPCDFile (PCD_FILE, cloud_blob) == -1)
00061   {
00062     ROS_ERROR ("Couldn't read pcd file.");
00063     return (-1);
00064   }
00065   ROS_INFO ("Loaded %d data points from pcd files", (int)(cloud_blob.width * cloud_blob.height));
00066 
00067   // Converting from PointCloud2 msg format to pcl pointcloud format
00068   pcl::fromROSMsg(cloud_blob, cloud);
00069 
00070   //KdTree stuff
00071   pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree;
00072   tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
00073   tree->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
00074 
00075   //indices stuff
00076   indices.resize(cloud.points.size());
00077   for(size_t i = 0; i < cloud.points.size(); i++)
00078   {
00079     indices[i] = i;
00080   }
00081   // Estimate normals first
00082   pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> n;
00083   pcl::PointCloud<pcl::PointNormal> normals;
00084   // set parameters
00085   n.setInputCloud (boost::make_shared <const pcl::PointCloud<pcl::PointXYZ> > (cloud));
00086   n.setIndices (boost::make_shared <std::vector<int> > (indices));
00087   n.setSearchMethod (tree);
00088   n.setKSearch (30);    // Use 10 nearest neighbors to estimate the normals
00089   // estimate
00090   n.compute (normals);
00091   ROS_INFO("Normals computed.");
00092 
00093   //Spin Image Estimation
00094   //s.setSearchSurface(boost::make_shared< pcl::PointCloud<pcl::PointXYZ> > (cloud));
00095   s.setInputNormals(boost::make_shared< pcl::PointCloud<pcl::PointNormal> > (normals));
00096   s.setInputCloud(boost::make_shared< pcl::PointCloud<pcl::PointXYZ> > (cloud));
00097   s.setIndices(boost::make_shared< std::vector<int> > (sil_indices));
00098   s.setSearchMethod(tree);
00099   s.setKSearch(MAX_NN);
00100   s.setNrSubdivisions(SUB_DIVISIONS);
00101   s.setRadius(RADIUS);
00102   s.compute(sil);
00103 
00104   ROS_INFO("Spin Image Estimation Complete.");
00105 
00106   //Write the histogram values of a selected point to a PPM file
00107   std::ofstream ppm_file;
00108 
00109   //std::cout<<sil.points.size()<<std::endl;
00110   for(size_t idx = 0 ; idx < sil_indices.size(); idx++)
00111   {
00112     std::stringstream ss;
00113     ss <<PCD_FILE<<"_"<<sil_indices[idx]<<".ppm";
00114     ppm_file.open((ss.str()).c_str());
00115     ppm_file << "P3"<<std::endl;
00116     ppm_file << "#Point: "<<sil_indices[idx]<<std::endl;
00117     ppm_file << SUB_DIVISIONS*10<<" " <<SUB_DIVISIONS*10<<std::endl;
00118     ppm_file << MAX_VALUE_PPM << std::endl;
00119 
00120     for(int row = 0; row < SUB_DIVISIONS; row ++)
00121     {
00122       for(int j = 0; j < 10; j++)
00123       {
00124         for(int col = 0; col < SUB_DIVISIONS; col ++)
00125         {
00126           int i = row*SUB_DIVISIONS + col;
00127           if( sil.points[idx].histogram[i] > MAX_VALUE_PPM)
00128           {
00129             ROS_WARN("Spin Image feature value larger than user defined max value");
00130             sil.points[idx].histogram[i] = MAX_VALUE_PPM;
00131           }
00132           for(int k =0; k < 10; k++)
00133           {
00134               ppm_file<<sil.points[idx].histogram[i] <<" ";
00135               ppm_file<<sil.points[idx].histogram[i] <<" ";
00136               ppm_file<<sil.points[idx].histogram[i] <<" ";
00137               ppm_file<<"    ";
00138           }
00139         }
00140 
00141         ppm_file << std::endl;
00142       }
00143     }
00144     ppm_file.close();
00145   }
00146   return 0;
00147 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pointcloud_registration
Author(s): Hozefa Indorewala
autogenerated on Sun Oct 6 2013 11:55:58