depth_image_triangulation.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu>, 
00003  Zoltan-Csaba Marton <marton@cs.tum.edu>, Nico Blodow <blodow@cs.tum.edu>
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  *     * Neither the name of Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <map>
00032 #include <iostream>
00033 #include <algorithm>
00034 
00035 #include <stdio.h>
00036 #include <stdlib.h>
00037 #include <float.h>
00038 #include <math.h>
00039 
00040 #include <ros/this_node.h>
00041 #include <pcl/io/pcd_io.h>
00042 
00043 #include <pcl_cloud_algos/cloud_algos.h>
00044 #include <pcl_cloud_algos/depth_image_triangulation.h>
00045 #include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>
00046 
00047 using namespace pcl;
00048 using namespace pcl_cloud_algos;
00049 
00051 void DepthImageTriangulation::get_scan_and_point_id (pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in)
00052 {
00053   int line_id = 0;
00054   int point_id = 0;
00055   int temp_point_id = 0;
00056 
00057   ros::Time ts = ros::Time::now ();
00058 
00059   // TODO try it w/ and w/o -1 //
00060   for (unsigned int k = 0; k < cloud_in.points.size()-1; k++)
00061   {
00062     cloud_in.points[k].line = line_id;
00063     point_id = cloud_in.points[k].index;
00064     temp_point_id = cloud_in.points[k+1].index;
00065     
00066     // new line found
00067     if (temp_point_id < point_id)
00068     {
00069       line_id++;
00070       // find max point index in the whole point cloud
00071       if (point_id > max_index_)
00072         max_index_ = point_id;
00073     }
00074   }
00075   
00076   // nr of lines in point cloud
00077   max_line_ = line_id;
00078 
00079   if (save_pcd_)
00080   {
00081     if (verbosity_level_ > 0) ROS_INFO ("Saving PCD containing line numbers as cloud_line.pcd in ROS home.");
00082     pcl::PCDWriter pcd_writer;
00083     pcd_writer.write ("cloud_line.pcd", cloud_in, false);
00084   }
00085 
00086 #ifdef DEBUG
00087   if (verbosity_level_ > 1) ROS_INFO ("Nr lines: %d, Max point ID: %d Completed in %f seconds", max_line_, max_index_, (ros::Time::now () - ts).toSec ());
00088 #endif
00089 }
00090 
00092 float DepthImageTriangulation::dist_3d (const pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in, int a, int b)
00093 {
00094   return sqrt( (cloud_in.points[a].x - cloud_in.points[b].x) * (cloud_in.points[a].x - cloud_in.points[b].x)
00095                + (cloud_in.points[a].y - cloud_in.points[b].y) * (cloud_in.points[a].y - cloud_in.points[b].y) 
00096                + (cloud_in.points[a].z - cloud_in.points[b].z) * (cloud_in.points[a].z - cloud_in.points[b].z) );
00097 }
00098 
00100 void DepthImageTriangulation::init (ros::NodeHandle &nh)
00101 {
00102   // node handler and publisher 
00103   nh_ = nh; 
00104   ROS_INFO ("Depth Image Triangulation Node initialized");
00105   nh_.param("save_pcd", save_pcd_, save_pcd_);
00106 }
00107 
00109 void DepthImageTriangulation::pre () 
00110   {
00111 
00112   }
00113 
00115 void DepthImageTriangulation::post ()
00116   {
00117  
00118   }
00119 
00121 std::vector<std::string> DepthImageTriangulation::requires () 
00122   {
00123     std::vector<std::string> requires;
00124     // requires 3D coordinates
00125     requires.push_back("x");
00126     requires.push_back("y");
00127     requires.push_back("z");
00128     // requires index
00129     requires.push_back("index");
00130     return requires;
00131   }
00132 
00134 std::vector<std::string> DepthImageTriangulation::provides ()
00135   {
00136     std::vector<std::string> provides;
00137     provides.push_back ("Triangled Mesh");
00138     return provides;
00139   }
00140 
00142 std::string DepthImageTriangulation::process (const boost::shared_ptr<const DepthImageTriangulation::InputType>& cloud_in)
00143 {
00144 #ifdef DEBUG
00145   if (verbosity_level_ > 1) ROS_INFO ("\n");
00146   if (verbosity_level_ > 1) ROS_INFO ("PointCloud msg with size %ld received", cloud_in->points.size());
00147 #endif
00148   
00149   // we assume here having either SICK LMS400 data which has line and index coeff
00150   // or
00151   // Hokuyo UTM 30LX which only returns index coeff, line has to be computed
00152 
00153   ros::Time ts = ros::Time::now ();
00154 
00155   // convert from sensor_msgs::PointCloud2 to pcl::PointCloud<T>
00156   fromROSMsg (*cloud_in, cloud_with_line_);
00157  
00158   // lock down the point cloud 
00159   boost::mutex::scoped_lock lock (cloud_lock_);
00160 
00161   // <cloud_in> gets processed into <cloud_with_line_> and <max_line_> with <max_index_> are computed in get_scan_and_point_id ()
00162   get_scan_and_point_id (cloud_with_line_);
00163 
00164   // print the size of point cloud 
00165   if (verbosity_level_ > 0) ROS_INFO ("max line: %d, max index: %d", max_line_, max_index_);
00166 
00167   std::vector<triangle> tr;
00168   tr.resize (2*max_line_*max_index_);  
00169   mesh_ = boost::shared_ptr<DepthImageTriangulation::OutputType>(new DepthImageTriangulation::OutputType);
00170   mesh_->points.resize (0);
00171   mesh_->triangles.resize (0);
00172   mesh_->intensities.resize (0);
00173 
00174   // number of triangles
00175   int nr = 0; 
00176   int nr_tr;
00177    
00178   int a = 0, b, c, d, e;
00179   bool skipped = false;
00180     
00181   // scan line in a point cloud 
00182   for (int i = 0; i <=  max_line_; i++)
00183   {
00184     // laser beam in a scan line
00185     for (int j = 0; j <= max_index_; j++)
00186     {
00187       if (cloud_with_line_.points[a].line == i && cloud_with_line_.points[a].index == j)
00188       {
00189         // ROS_INFO ("found point a = %d\n", a);
00190 
00191         b = -1;
00192         c = -1;
00193         d = -1;
00194         e = -1;
00195         
00196         // find top right corner
00197         if ((unsigned int)a+1 < cloud_with_line_.points.size() && cloud_with_line_.points[a+1].line == i && cloud_with_line_.points[a+1].index == j+1)
00198           b = a+1;
00199 
00200         // ROS_INFO ("resolved point b = %d\n", b);
00201 
00202         // go to next line
00203         int test = a;
00204         while ((unsigned int)test < cloud_with_line_.points.size() &&  cloud_with_line_.points[test].line < i+1)
00205           test++;
00206 
00207         // ROS_INFO ("resolved next line\n");
00208 
00209         // if next line exists
00210         if ((unsigned int)test < cloud_with_line_.points.size() && cloud_with_line_.points[test].line == i+1)
00211         {
00212           // a skipped triangle exists because of missing 'a'
00213           if (skipped)
00214           {
00215             // reset var
00216             skipped = false;
00217 
00218             // go to column j-1
00219             while ((unsigned int)test < cloud_with_line_.points.size() &&  cloud_with_line_.points[test].index < j-1)
00220               test++;
00221 
00222             // if not at the end of dataset
00223             if ((unsigned int)test <  cloud_with_line_.points.size())
00224             {
00225               // if column exists
00226               if (cloud_with_line_.points[test].line == i+1 && cloud_with_line_.points[test].index)
00227               {
00228                 e = test;
00229                 test++;
00230               }
00231             }
00232           }
00233           else
00234           {
00235             // go to column j
00236             while ((unsigned int)test < cloud_with_line_.points.size() && cloud_with_line_.points[test].index < j)
00237               test++;
00238           }
00239 
00240           // if not at the end of dataset
00241           if ((unsigned int)test < cloud_with_line_.points.size())
00242           {
00243           // if column exists
00244             if (cloud_with_line_.points[test].line == i+1 && cloud_with_line_.points[test].index == j)
00245             {
00246               c = test;
00247               if ((unsigned int)c+1 < cloud_with_line_.points.size() && cloud_with_line_.points[c+1].line == i+1 && cloud_with_line_.points[c+1].index == j+1)
00248                 d = c+1;
00249             }
00250             // if next column was found
00251             else if (cloud_with_line_.points[test].line == i+1 && cloud_with_line_.points[test].index == j+1)
00252               d = test;
00253           }
00254         }
00255 
00256         if (c != -1)
00257         {
00258           float AC = dist_3d (cloud_with_line_, a, c);
00259           if (e != -1)
00260           {
00261             // a c e
00262             // ROS_INFO ("a c e\n");
00263             float AE = dist_3d (cloud_with_line_, a, e);
00264             float CE = dist_3d (cloud_with_line_, c, e);
00265             if (AC < max_length && CE < max_length && AE < max_length)
00266             {
00267               tr[nr].a = a;
00268               tr[nr].b = c;
00269               tr[nr].c = e;
00270               nr++;
00271             }
00272           }
00273 
00274           if (b != -1)
00275           {
00276             // a b c
00277             // ROS_INFO ("a b c\n");
00278             float AB = dist_3d (cloud_with_line_, a, b);
00279             float BC = dist_3d (cloud_with_line_, b, c);
00280             float AC = dist_3d (cloud_with_line_, a, c);
00281             if (AB < max_length && BC < max_length && AC < max_length)
00282             {
00283               tr[nr].a = a;
00284               tr[nr].b = b;
00285               tr[nr].c = c;
00286               nr++;
00287             }
00288 
00289             if (d != -1)
00290             {
00291               // b c d
00292               // ROS_INFO ("b c d\n");
00293               float BD = dist_3d (cloud_with_line_, b, d);
00294               float CD = dist_3d (cloud_with_line_, c, d);
00295               if (BD < max_length && BC < max_length && CD < max_length)
00296               {
00297                 tr[nr].a = b;
00298                 tr[nr].b = c;
00299                 tr[nr].c = d;
00300                 nr++;
00301               }
00302             }
00303           }
00304           else if (d != -1)
00305           {
00306             // a c d
00307             // ROS_INFO ("a c d\n");
00308             float AD = dist_3d (cloud_with_line_, a, d);
00309             float CD = dist_3d (cloud_with_line_, c, d);
00310             if (AD < max_length && CD < max_length && AC < max_length)
00311             {
00312               tr[nr].a = a;
00313               tr[nr].b = c;
00314               tr[nr].c = d;
00315               nr++;
00316             }
00317           }
00318         }
00319         else if (b != -1 && d != -1)
00320         {
00321           // a b d
00322           // ROS_INFO ("a b d\n");
00323           float AB = dist_3d (cloud_with_line_, a, b);
00324           float AD = dist_3d (cloud_with_line_, a, d);
00325           float BD = dist_3d (cloud_with_line_, b, d);
00326           if (AD < max_length && BD < max_length && AB < max_length)
00327           {
00328             tr[nr].a = a;
00329             tr[nr].b = b;
00330             tr[nr].c = d;
00331             nr++;
00332           }
00333         }
00334 
00335         // move to next point
00336         a++;
00337 
00338         // skipped = false;
00339         if ((unsigned int)a >= cloud_with_line_.points.size())
00340           break;
00341       } // END OF : top left corner found 
00342       else
00343         skipped = true;
00344       } // END OF : number of indexes 
00345     if ((unsigned int)a >= cloud_with_line_.points.size())
00346       break;
00347   } // END OF : number of lines 
00348 
00349   nr_tr = nr;
00350   tr.resize(nr);
00351   mesh_->header = cloud_with_line_.header;   
00352   mesh_->sending_node = ros::this_node::getName();   
00353   triangle_mesh_msgs::Triangle tr_mesh;
00354 
00355 #ifdef DEBUG  
00356   if (verbosity_level_ > 1) ROS_INFO ("Triangle a: %d, b: %d, c: %d", tr[i].a, tr[i].b, tr[i].c);
00357 #endif
00358 
00359   // fill up triangular mesh msg and send it on the topic
00360   geometry_msgs::Point32 p;
00361   for (unsigned int i = 0; i < cloud_with_line_.points.size(); i++)
00362   {
00363     p.x = cloud_with_line_.points[i].x;
00364     p.y = cloud_with_line_.points[i].y;
00365     p.z = cloud_with_line_.points[i].z;
00366     mesh_->points.push_back (p);
00367   }
00368   
00369   for (int i = 0; i < nr; i++)
00370   {
00371     if ((unsigned int)tr[i].a >= cloud_with_line_.points.size() || tr[i].a < 0 || isnan(tr[i].a)); 
00372       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, tr[i].a, tr[i].b, tr[i].c, cloud_with_line_.points.size());
00373     else if ((unsigned int)tr[i].b >= cloud_with_line_.points.size() || tr[i].b < 0 || isnan(tr[i].b));
00374       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, tr[i].a, tr[i].b, tr[i].c, cloud_with_line_.points.size());
00375     else if ((unsigned int)tr[i].c >= cloud_with_line_.points.size() || tr[i].c < 0 || isnan(tr[i].c)); 
00376       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, tr[i].a, tr[i].b, tr[i].c, cloud_with_line_.points.size());
00377     else if (tr[i].a == tr[i].b || tr[i].a == tr[i].c || tr[i].b == tr[i].c);
00378       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, tr[i].a, tr[i].b, tr[i].c, cloud_with_line_.points.size());
00379     else
00380     {
00381       tr_mesh.i = tr[i].a;
00382       tr_mesh.j = tr[i].c;
00383       tr_mesh.k = tr[i].b;
00384       mesh_->triangles.push_back(tr_mesh);
00385     } 
00386   }
00387 
00388   // fill in intensities (needed e.g. laser-to-camera calibration)
00389   std::vector<sensor_msgs::PointField> fields;
00390   int iIdx = getFieldIndex (cloud_with_line_, "intensities", fields);
00391   if (iIdx == -1)
00392   {
00393     if (verbosity_level_ > -1) ROS_WARN ("[DepthImageTriangulaton] \"intensities\" channel does not exist");
00394   }
00395   else
00396   {
00397     mesh_->intensities.resize (cloud_with_line_.points.size());
00398     for (unsigned int i = 0; i < cloud_with_line_.points.size(); i++)
00399       mesh_->intensities[i] = cloud_with_line_.points[i].intensities;
00400   }
00401 
00402   // write to vtk file for display in e.g. vtk viewer
00403   if (write_to_vtk_)
00404     write_vtk_file ("data/triangles.vtk", tr, cloud_with_line_, nr);
00405   if (verbosity_level_ > 0) ROS_INFO ("Triangulation with %d triangles completed in %g seconds", tr.size(), (ros::Time::now() - ts).toSec());
00406   return std::string("");
00407 }
00408 
00410 void DepthImageTriangulation::write_vtk_file (std::string output, std::vector<triangle> triangles, const pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in, int nr_tr)
00411 {
00412   /* writing VTK file */
00413 
00414   FILE *f;
00415   f = fopen(output.c_str(),"w");
00416   fprintf (f, "# vtk DataFile Version 3.0\nvtk output\nASCII\nDATASET POLYDATA\nPOINTS %d float\n", cloud_with_line_.points.size());
00417 
00418   for (unsigned int i = 0; i < cloud_with_line_.points.size(); i++)
00419   {
00420     fprintf (f,"%f %f %f ", cloud_with_line_.points[i].x, cloud_with_line_.points[i].y,
00421         cloud_with_line_.points[i].z);
00422     fprintf (f,"\n");
00423   }
00424 
00425   fprintf(f,"VERTICES %d %d\n", cloud_with_line_.points.size(), 2*cloud_with_line_.points.size());
00426   for (unsigned int i = 0; i < cloud_with_line_.points.size(); i++)
00427     fprintf(f,"1 %d\n", i);
00428 
00429   if (verbosity_level_ > 0) ROS_INFO ("vector: %d, nr: %d  ", triangles.size(), nr_tr);
00430 
00431   fprintf(f,"\nPOLYGONS %d %d\n", nr_tr, 4*nr_tr);
00432   for (int i = 0; i < nr_tr; i++)
00433   {
00434     if ((unsigned int)triangles[i].a >= cloud_with_line_.points.size() || triangles[i].a < 0 || isnan(triangles[i].a));
00435       //printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, triangles[i].a, triangles[i].b, triangles[i].c, cloud_with_line_.points.size());
00436     else if ((unsigned int)triangles[i].b >= cloud_with_line_.points.size() || triangles[i].b < 0 || isnan(triangles[i].b));
00437       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, triangles[i].a, triangles[i].b, triangles[i].c, cloud_with_line_.points.size());
00438     else if ((unsigned int)triangles[i].c >= cloud_with_line_.points.size() || triangles[i].c < 0 || isnan(triangles[i].c));
00439       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, triangles[i].a, triangles[i].b, triangles[i].c, cloud_with_line_.points.size());
00440     else if (triangles[i].a == triangles[i].b || triangles[i].a == triangles[i].c || triangles[i].b == triangles[i].c);
00441       // printf("triangle %d/%d: %d %d %d / %d\n", i, nr_tr, triangles[i].a, triangles[i].b, triangles[i].c, cloud_with_line_.points.size());
00442     else
00443       fprintf(f,"3 %d %d %d\n",triangles[i].a, triangles[i].c, triangles[i].b);
00444   }
00445 }
00446 
00448 boost::shared_ptr<const DepthImageTriangulation::OutputType> DepthImageTriangulation::output ()
00449   {
00450     return mesh_;
00451   }
00452 
00453 
00454 #ifdef CREATE_NODE
00455 int main (int argc, char* argv[])
00456 {
00457   return standalone_node <DepthImageTriangulation> (argc, argv);
00458 }
00459 #endif
00460 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:48