00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00052
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
00056 cloud_out.header = scan_in.header;
00057 cloud_out.points.resize (scan_in.ranges.size());
00058
00059
00060 int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
00061
00062 cloud_out.channels.resize(0);
00063
00064
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
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
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)))
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
00117
00118
00119
00120
00121
00122
00123 if (idx_index != -1)
00124 cloud_out.channels[idx_index].values[count] = index;
00125
00126
00127 if (idx_distance != -1)
00128 cloud_out.channels[idx_distance].values[count] = range;
00129
00130
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
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
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
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
00161 if (it != unit_vector_map_.end())
00162 return *((*it).second);
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
00171 unit_vector_map_[anglestring.str()] = tempPtr;
00172
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
00198 bool requested_index = false;
00199 if ((mask & channel_option::Index))
00200 requested_index = true;
00201
00202
00203
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
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
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
00236 ROS_ASSERT(index_channel_idx >= 0);
00237
00238 for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
00239 {
00240
00241 uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
00242
00243
00244 tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
00245
00247
00248
00249 tf::Vector3 v (0, 0, 0);
00250 v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
00251 cur_transform.setOrigin(v) ;
00252
00253
00254 tf::Quaternion q1, q2 ;
00255 start_transform.getBasis().getRotation(q1) ;
00256 end_transform.getBasis().getRotation(q2) ;
00257
00258
00259 cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
00260
00261
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
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
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
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
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
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
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
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
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
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
00426 pstep[0] = output (i, 0);
00427 pstep[1] = output (i, 1);
00428 pstep[2] = 0;
00429
00430
00431 if(idx_intensity != -1)
00432 pstep[idx_intensity] = scan_in.intensities[i];
00433
00434
00435 if(idx_index != -1)
00436 ((int*)(pstep))[idx_index] = i;
00437
00438
00439 if(idx_distance != -1)
00440 pstep[idx_distance] = range;
00441
00442
00443 if(idx_timestamp != -1)
00444 pstep[idx_timestamp] = i * scan_in.time_increment;
00445
00446
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
00455 ++count;
00456 }
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488 }
00489
00490
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
00507 bool requested_index = false;
00508 if ((channel_options & channel_option::Index))
00509 requested_index = true;
00510
00511
00512
00513 channel_options |= channel_option::Index;
00514
00515 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00516
00517
00518 bool has_viewpoint = false;
00519 uint32_t vp_x_offset = 0;
00520
00521
00522
00523
00524
00525
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
00535
00536
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
00553 for(size_t i = 0; i < cloud_out.width; ++i)
00554 {
00555
00556 float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
00557
00558
00559 uint32_t pt_index;
00560 memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
00561
00562
00563 tfScalar ratio = pt_index * ranges_norm;
00564
00566
00567 tf2::Vector3 v (0, 0, 0);
00568 v.setInterpolate3 (origin_start, origin_end, ratio);
00569 cur_transform.setOrigin (v);
00570
00571
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
00578 pstep[0] = point_out.x ();
00579 pstep[1] = point_out.y ();
00580 pstep[2] = point_out.z ();
00581
00582
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
00590 vpstep[0] = point_out.x ();
00591 vpstep[1] = point_out.y ();
00592 vpstep[2] = point_out.z ();
00593 }
00594 }
00595
00596
00597 if(!requested_index)
00598 {
00599 sensor_msgs::PointCloud2 cloud_without_index;
00600
00601
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
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
00623 offset_shift = 4;
00624 }
00625 }
00626
00627
00628 cloud_without_index.fields.resize(field_count);
00629
00630
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
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
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 }