laser_geometry.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
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 the 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 #include "laser_geometry/laser_geometry.h"
00031 #include <algorithm>
00032 #include <ros/assert.h>
00033 
00034 namespace laser_geometry
00035 {
00036 
00037   void
00038     LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, 
00039                                    bool preservative, int mask)
00040   {
00041     boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
00042 
00043     // Fill the ranges matrix
00044     for (unsigned int index = 0; index < scan_in.ranges.size(); index++)
00045       {
00046         ranges(0,index) = (double) scan_in.ranges[index];
00047         ranges(1,index) = (double) scan_in.ranges[index];
00048       }
00049     
00050 
00051     //Do the projection
00052     //    NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
00053     boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
00054 
00055     //Stuff the output cloud
00056     cloud_out.header = scan_in.header;
00057     cloud_out.points.resize (scan_in.ranges.size());
00058 
00059     // Define 4 indices in the channel array for each possible value type
00060     int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
00061 
00062     cloud_out.channels.resize(0);    
00063 
00064     // Check if the intensity bit is set
00065     if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
00066     {
00067       int chan_size = cloud_out.channels.size();
00068       cloud_out.channels.resize (chan_size + 1);
00069       cloud_out.channels[0].name = "intensities";
00070       cloud_out.channels[0].values.resize (scan_in.intensities.size());
00071       idx_intensity = 0;
00072     }
00073     
00074     // Check if the index bit is set
00075     if (mask & channel_option::Index)
00076     {
00077       int chan_size = cloud_out.channels.size();
00078       cloud_out.channels.resize (chan_size +1);
00079       cloud_out.channels[chan_size].name = "index";
00080       cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00081       idx_index = chan_size;
00082     }
00083 
00084     // Check if the distance bit is set
00085     if (mask & channel_option::Distance)
00086     {
00087       int chan_size = cloud_out.channels.size();
00088       cloud_out.channels.resize (chan_size + 1);
00089       cloud_out.channels[chan_size].name = "distances";
00090       cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00091       idx_distance = chan_size;
00092     }
00093 
00094     if (mask & channel_option::Timestamp)
00095     {
00096       int chan_size = cloud_out.channels.size();
00097       cloud_out.channels.resize (chan_size + 1);
00098       cloud_out.channels[chan_size].name = "stamps";
00099       cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00100       idx_timestamp = chan_size;
00101     }
00102 
00103     if (range_cutoff < 0)
00104       range_cutoff = scan_in.range_max;
00105     else
00106       range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
00107     
00108     unsigned int count = 0;
00109     for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
00110     {
00111       if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative
00112       {
00113           
00114         cloud_out.points[count].x = output(0,index);
00115         cloud_out.points[count].y = output(1,index);
00116         cloud_out.points[count].z = 0.0;
00117           
00118         //double x = cloud_out.points[count].x;
00119         //double y = cloud_out.points[count].y;
00120         //if(x*x + y*y < scan_in.range_min * scan_in.range_min){
00121         //  ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y);
00122         //}
00123 
00124         // Save the original point index
00125         if (idx_index != -1)
00126           cloud_out.channels[idx_index].values[count] = index;
00127 
00128         // Save the original point distance
00129         if (idx_distance != -1)
00130           cloud_out.channels[idx_distance].values[count] = ranges (0, index);
00131 
00132         // Save intensities channel
00133         if (scan_in.intensities.size() >= index)
00134         { 
00135           if (idx_intensity != -1)
00136             cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
00137         }
00138 
00139         // Save timestamps to seperate channel if asked for
00140         if( idx_timestamp != -1)
00141                   cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
00142 
00143         count++;
00144       }
00145     }
00146    
00147 
00148     //downsize if necessary
00149     cloud_out.points.resize (count);
00150     for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
00151       cloud_out.channels[d].values.resize(count);
00152   };
00153 
00154 const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
00155   {
00156     boost::mutex::scoped_lock guv_lock(this->guv_mutex_);
00157 
00158     //construct string for lookup in the map
00159     std::stringstream anglestring;
00160     anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
00161     std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
00162     it = unit_vector_map_.find(anglestring.str());
00163     //check the map for presense
00164     if (it != unit_vector_map_.end())
00165       return *((*it).second);     //if present return
00166 
00167     boost::numeric::ublas::matrix<double> * tempPtr = new boost::numeric::ublas::matrix<double>(2,length);
00168     for (unsigned int index = 0;index < length; index++)
00169       {
00170         (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
00171         (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
00172       }
00173     //store 
00174     unit_vector_map_[anglestring.str()] = tempPtr;
00175     //and return
00176     return *tempPtr;
00177   };
00178 
00179 
00180   LaserProjection::~LaserProjection()
00181   {
00182     std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
00183     it = unit_vector_map_.begin();
00184     while (it != unit_vector_map_.end())
00185       {
00186         delete (*it).second;
00187         it++;
00188       }
00189   };
00190 
00191   void
00192     LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
00193                                                      tf::Transformer& tf, double range_cutoff, int mask)
00194   {
00195     cloud_out.header = scan_in.header;
00196 
00197     tf::Stamped<tf::Point> pointIn;
00198     tf::Stamped<tf::Point> pointOut;
00199 
00200     //check if the user has requested the index field
00201     bool requested_index = false;
00202     if ((mask & channel_option::Index))
00203       requested_index = true;
00204 
00205     //we need to make sure that we include the index in our mask
00206     //in order to guarantee that we get our timestamps right
00207     mask |= channel_option::Index;
00208 
00209     pointIn.frame_id_ = scan_in.header.frame_id;
00210 
00211     projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask);
00212 
00213     cloud_out.header.frame_id = target_frame;
00214 
00215     // Extract transforms for the beginning and end of the laser scan
00216     ros::Time start_time = scan_in.header.stamp ;
00217     ros::Time end_time   = scan_in.header.stamp + ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment);
00218 
00219     tf::StampedTransform start_transform ;
00220     tf::StampedTransform end_transform ;
00221     tf::StampedTransform cur_transform ;
00222 
00223     tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
00224     tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
00225 
00226     //we need to find the index of the index channel
00227     int index_channel_idx = -1;
00228     for(unsigned int i = 0; i < cloud_out.channels.size(); ++i)
00229     {
00230       if(cloud_out.channels[i].name == "index")
00231       {
00232         index_channel_idx = i;
00233         break;
00234       }
00235     }
00236 
00237     //check just in case
00238     ROS_ASSERT(index_channel_idx >= 0);
00239 
00240     for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
00241     {
00242       //get the index for this point
00243       uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
00244 
00245       // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
00246       tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
00247 
00249 
00250       //Interpolate translation
00251       tf::Vector3 v (0, 0, 0);
00252       v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
00253       cur_transform.setOrigin(v) ;
00254 
00255       //Interpolate rotation
00256       tf::Quaternion q1, q2 ;
00257       start_transform.getBasis().getRotation(q1) ;
00258       end_transform.getBasis().getRotation(q2) ;
00259 
00260       // Compute the slerp-ed rotation
00261       cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
00262 
00263       // Apply the transform to the current point
00264       tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
00265       tf::Vector3 pointOut = cur_transform * pointIn ;
00266 
00267       // Copy transformed point into cloud
00268       cloud_out.points[i].x  = pointOut.x();
00269       cloud_out.points[i].y  = pointOut.y();
00270       cloud_out.points[i].z  = pointOut.z();
00271     }
00272 
00273     //if the user didn't request the index, we want to remove it from the channels
00274     if(!requested_index)
00275       cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx);
00276   }
00277 
00278   void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, 
00279                                       sensor_msgs::PointCloud2 &cloud_out,
00280                                       double range_cutoff,
00281                                       int channel_options)
00282   {
00283     size_t n_pts = scan_in.ranges.size ();
00284     Eigen::ArrayXXd ranges (n_pts, 2);
00285     Eigen::ArrayXXd output (n_pts, 2);
00286 
00287     // Get the ranges into Eigen format
00288     for (size_t i = 0; i < n_pts; ++i)
00289     {
00290       ranges (i, 0) = (double) scan_in.ranges[i];
00291       ranges (i, 1) = (double) scan_in.ranges[i];
00292     }
00293 
00294     // Check if our existing co_sine_map is valid
00295     if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max )
00296     {
00297       ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
00298       co_sine_map_ = Eigen::ArrayXXd (n_pts, 2);
00299       angle_min_ = scan_in.angle_min;
00300       angle_max_ = scan_in.angle_max;
00301       // Spherical->Cartesian projection
00302       for (size_t i = 0; i < n_pts; ++i)
00303       {
00304         co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment);
00305         co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
00306       }
00307     }
00308 
00309     output = ranges * co_sine_map_;
00310 
00311     // Set the output cloud accordingly
00312     cloud_out.header = scan_in.header;
00313     cloud_out.height = 1;
00314     cloud_out.width  = scan_in.ranges.size ();
00315     cloud_out.fields.resize (3);
00316     cloud_out.fields[0].name = "x";
00317     cloud_out.fields[0].offset = 0;
00318     cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00319     cloud_out.fields[0].count = 1;
00320     cloud_out.fields[1].name = "y";
00321     cloud_out.fields[1].offset = 4;
00322     cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00323     cloud_out.fields[1].count = 1;
00324     cloud_out.fields[2].name = "z";
00325     cloud_out.fields[2].offset = 8;
00326     cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00327     cloud_out.fields[2].count = 1;
00328 
00329     // Define 4 indices in the channel array for each possible value type
00330     int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
00331 
00332     //now, we need to check what fields we need to store
00333     int offset = 12;
00334     if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
00335     {
00336       int field_size = cloud_out.fields.size();
00337       cloud_out.fields.resize(field_size + 1);
00338       cloud_out.fields[field_size].name = "intensity";
00339       cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00340       cloud_out.fields[field_size].offset = offset;
00341       cloud_out.fields[field_size].count = 1;
00342       offset += 4;
00343       idx_intensity = field_size;
00344     }
00345 
00346     if ((channel_options & channel_option::Index))
00347     {
00348       int field_size = cloud_out.fields.size();
00349       cloud_out.fields.resize(field_size + 1);
00350       cloud_out.fields[field_size].name = "index";
00351       cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
00352       cloud_out.fields[field_size].offset = offset;
00353       cloud_out.fields[field_size].count = 1;
00354       offset += 4;
00355       idx_index = field_size;
00356     }
00357 
00358     if ((channel_options & channel_option::Distance))
00359     {
00360       int field_size = cloud_out.fields.size();
00361       cloud_out.fields.resize(field_size + 1);
00362       cloud_out.fields[field_size].name = "distances";
00363       cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00364       cloud_out.fields[field_size].offset = offset;
00365       cloud_out.fields[field_size].count = 1;
00366       offset += 4;
00367       idx_distance = field_size;
00368     }
00369 
00370     if ((channel_options & channel_option::Timestamp))
00371     {
00372       int field_size = cloud_out.fields.size();
00373       cloud_out.fields.resize(field_size + 1);
00374       cloud_out.fields[field_size].name = "stamps";
00375       cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00376       cloud_out.fields[field_size].offset = offset;
00377       cloud_out.fields[field_size].count = 1;
00378       offset += 4;
00379       idx_timestamp = field_size;
00380     }
00381 
00382     if ((channel_options & channel_option::Viewpoint))
00383     {
00384       int field_size = cloud_out.fields.size();
00385       cloud_out.fields.resize(field_size + 3);
00386 
00387       cloud_out.fields[field_size].name = "vp_x";
00388       cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00389       cloud_out.fields[field_size].offset = offset;
00390       cloud_out.fields[field_size].count = 1;
00391       offset += 4;
00392 
00393       cloud_out.fields[field_size + 1].name = "vp_y";
00394       cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
00395       cloud_out.fields[field_size + 1].offset = offset;
00396       cloud_out.fields[field_size + 1].count = 1;
00397       offset += 4;
00398 
00399       cloud_out.fields[field_size + 2].name = "vp_z";
00400       cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
00401       cloud_out.fields[field_size + 2].offset = offset;
00402       cloud_out.fields[field_size + 2].count = 1;
00403       offset += 4;
00404 
00405       idx_vpx = field_size;
00406       idx_vpy = field_size + 1;
00407       idx_vpz = field_size + 2;
00408     }
00409 
00410     cloud_out.point_step = offset;
00411     cloud_out.row_step   = cloud_out.point_step * cloud_out.width;
00412     cloud_out.data.resize (cloud_out.row_step   * cloud_out.height);
00413     cloud_out.is_dense = false;
00414 
00415     //TODO: Find out why this was needed
00416     //float bad_point = std::numeric_limits<float>::quiet_NaN ();
00417 
00418     if (range_cutoff < 0)
00419       range_cutoff = scan_in.range_max;
00420     else
00421       range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
00422 
00423     unsigned int count = 0;
00424     for (size_t i = 0; i < n_pts; ++i)
00425     {
00426       //check to see if we want to keep the point
00427       if (scan_in.ranges[i] < range_cutoff && scan_in.ranges[i] >= scan_in.range_min)
00428       {
00429         float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
00430 
00431         // Copy XYZ
00432         pstep[0] = output (i, 0);
00433         pstep[1] = output (i, 1);
00434         pstep[2] = 0;
00435 
00436         // Copy intensity
00437         if(idx_intensity != -1)
00438           pstep[idx_intensity] = scan_in.intensities[i];
00439 
00440         //Copy index
00441         if(idx_index != -1)
00442           ((int*)(pstep))[idx_index] = i;
00443 
00444         // Copy distance
00445         if(idx_distance != -1)
00446           pstep[idx_distance] = scan_in.ranges[i];
00447 
00448         // Copy timestamp
00449         if(idx_timestamp != -1)
00450           pstep[idx_timestamp] = i * scan_in.time_increment;
00451 
00452         // Copy viewpoint (0, 0, 0)
00453         if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
00454         {
00455           pstep[idx_vpx] = 0;
00456           pstep[idx_vpy] = 0;
00457           pstep[idx_vpz] = 0;
00458         }
00459 
00460         //make sure to increment count
00461         ++count;
00462       }
00463 
00464       /* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values
00465        * why can't you just leave them out?
00466        *
00467       // Invalid measurement?
00468       if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
00469       {
00470         if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE)
00471         {
00472           for (size_t s = 0; s < cloud_out.fields.size (); ++s)
00473             pstep[s] = bad_point;
00474         }
00475         else
00476         {
00477           // Kind of nasty thing:
00478           //   We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid.
00479           //   Since we still might need the x value we store it in the distance field
00480           pstep[0] = bad_point;           // X -> NAN to mark a bad point
00481           pstep[1] = co_sine_map (i, 1);  // Y
00482           pstep[2] = 0;                   // Z
00483 
00484           if (store_intensity)
00485           {
00486             pstep[3] = bad_point;           // Intensity -> NAN to mark a bad point
00487             pstep[4] = co_sine_map (i, 0);  // Distance -> Misused to store the originnal X
00488           }
00489           else
00490             pstep[3] = co_sine_map (i, 0);  // Distance -> Misused to store the originnal X
00491         }
00492       }
00493       */
00494     }
00495 
00496     //resize if necessary
00497     cloud_out.width = count;
00498     cloud_out.row_step   = cloud_out.point_step * cloud_out.width;
00499     cloud_out.data.resize (cloud_out.row_step   * cloud_out.height);
00500   }
00501 
00502   void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, 
00503                                                         const sensor_msgs::LaserScan &scan_in,
00504                                                         sensor_msgs::PointCloud2 &cloud_out, 
00505                                                         tf::Transformer &tf, 
00506                                                         double range_cutoff,
00507                                                         int channel_options)
00508   {
00509     //check if the user has requested the index field
00510     bool requested_index = false;
00511     if ((channel_options & channel_option::Index))
00512       requested_index = true;
00513 
00514     //we'll enforce that we get index values for the laser scan so that we
00515     //ensure that we use the correct timestamps
00516     channel_options |= channel_option::Index;
00517 
00518     projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00519 
00520     //we'll assume no associated viewpoint by default
00521     bool has_viewpoint = false;
00522     uint32_t vp_x_offset = 0;
00523 
00524     //we need to find the offset of the intensity field in the point cloud
00525     //we also know that the index field is guaranteed to exist since we 
00526     //set the channel option above. To be really safe, it might be worth
00527     //putting in a check at some point, but I'm just going to put in an
00528     //assert for now
00529     uint32_t index_offset = 0;
00530     for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
00531     {
00532       if(cloud_out.fields[i].name == "index")
00533       {
00534         index_offset = cloud_out.fields[i].offset;
00535       }
00536 
00537       //we want to check if the cloud has a viewpoint associated with it
00538       //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
00539       //get put in together
00540       if(cloud_out.fields[i].name == "vp_x")
00541       {
00542         has_viewpoint = true;
00543         vp_x_offset = cloud_out.fields[i].offset;
00544       }
00545     }
00546 
00547     ROS_ASSERT(index_offset > 0);
00548 
00549     cloud_out.header.frame_id = target_frame;
00550 
00551     // Extract transforms for the beginning and end of the laser scan
00552     ros::Time start_time = scan_in.header.stamp;
00553     ros::Time end_time   = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
00554 
00555     tf::StampedTransform start_transform, end_transform, cur_transform ;
00556 
00557     tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
00558     tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
00559 
00560     double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
00561 
00562     //we want to loop through all the points in the cloud
00563     for(size_t i = 0; i < cloud_out.width; ++i)
00564     {
00565       // Apply the transform to the current point
00566       float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
00567 
00568       //find the index of the point
00569       uint32_t pt_index;
00570       memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
00571       
00572       // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
00573       tfScalar ratio = pt_index * ranges_norm;
00574 
00576       // Interpolate translation
00577       tf::Vector3 v (0, 0, 0);
00578       v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
00579       cur_transform.setOrigin (v);
00580 
00581       // Interpolate rotation
00582       tf::Quaternion q1, q2;
00583       start_transform.getBasis ().getRotation (q1);
00584       end_transform.getBasis ().getRotation (q2);
00585 
00586       // Compute the slerp-ed rotation
00587       cur_transform.setRotation (slerp (q1, q2 , ratio));
00588 
00589       tf::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
00590       tf::Vector3 point_out = cur_transform * point_in;
00591 
00592       // Copy transformed point into cloud
00593       pstep[0] = point_out.x ();
00594       pstep[1] = point_out.y ();
00595       pstep[2] = point_out.z ();
00596 
00597       // Convert the viewpoint as well
00598       if(has_viewpoint)
00599       {
00600         float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
00601         point_in = tf::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
00602         point_out = cur_transform * point_in;
00603 
00604         // Copy transformed point into cloud
00605         vpstep[0] = point_out.x ();
00606         vpstep[1] = point_out.y ();
00607         vpstep[2] = point_out.z ();
00608       }
00609     }
00610 
00611     //if the user didn't request the index field, then we need to copy the PointCloud and drop it
00612     if(!requested_index)
00613     {
00614       sensor_msgs::PointCloud2 cloud_without_index;
00615 
00616       //copy basic meta data
00617       cloud_without_index.header = cloud_out.header;
00618       cloud_without_index.width = cloud_out.width;
00619       cloud_without_index.height = cloud_out.height;
00620       cloud_without_index.is_bigendian = cloud_out.is_bigendian;
00621       cloud_without_index.is_dense = cloud_out.is_dense;
00622 
00623       //copy the fields
00624       cloud_without_index.fields.resize(cloud_out.fields.size());
00625       unsigned int field_count = 0;
00626       unsigned int offset_shift = 0;
00627       for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
00628       {
00629         if(cloud_out.fields[i].name != "index")
00630         {
00631           cloud_without_index.fields[field_count] = cloud_out.fields[i];
00632           cloud_without_index.fields[field_count].offset -= offset_shift;
00633           ++field_count;
00634         }
00635         else
00636         {
00637           //once we hit the index, we'll set the shift
00638           offset_shift = 4;
00639         }
00640       }
00641 
00642       //resize the fields
00643       cloud_without_index.fields.resize(field_count);
00644 
00645       //compute the size of the new data
00646       cloud_without_index.point_step = cloud_out.point_step - offset_shift;
00647       cloud_without_index.row_step   = cloud_without_index.point_step * cloud_without_index.width;
00648       cloud_without_index.data.resize (cloud_without_index.row_step   * cloud_without_index.height);
00649 
00650       uint32_t i = 0;
00651       uint32_t j = 0;
00652       //copy over the data from one cloud to the other
00653       while (i < cloud_out.data.size())
00654       {
00655         if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4))
00656         {
00657           cloud_without_index.data[j++] = cloud_out.data[i];
00658         }
00659         i++;
00660       }
00661 
00662       //make sure to actually set the output
00663       cloud_out = cloud_without_index;
00664     }
00665   }
00666 
00667 
00668 } //laser_geometry


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Mon Oct 6 2014 01:47:27