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


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Tue Jul 2 2019 20:04:25