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
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
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
00052 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()));
00053
00054
00055 cloud_out.header = scan_in.header;
00056 cloud_out.points.resize (scan_in.ranges.size());
00057
00058
00059 int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
00060
00061 cloud_out.channels.resize(0);
00062
00063
00064 if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
00065 {
00066 int chan_size = cloud_out.channels.size();
00067 cloud_out.channels.resize (chan_size + 1);
00068 cloud_out.channels[0].name = "intensities";
00069 cloud_out.channels[0].values.resize (scan_in.intensities.size());
00070 idx_intensity = 0;
00071 }
00072
00073
00074 if (mask & channel_option::Index)
00075 {
00076 int chan_size = cloud_out.channels.size();
00077 cloud_out.channels.resize (chan_size +1);
00078 cloud_out.channels[chan_size].name = "index";
00079 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00080 idx_index = chan_size;
00081 }
00082
00083
00084 if (mask & channel_option::Distance)
00085 {
00086 int chan_size = cloud_out.channels.size();
00087 cloud_out.channels.resize (chan_size + 1);
00088 cloud_out.channels[chan_size].name = "distances";
00089 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00090 idx_distance = chan_size;
00091 }
00092
00093 if (mask & channel_option::Timestamp)
00094 {
00095 int chan_size = cloud_out.channels.size();
00096 cloud_out.channels.resize (chan_size + 1);
00097 cloud_out.channels[chan_size].name = "stamps";
00098 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
00099 idx_timestamp = chan_size;
00100 }
00101
00102 if (range_cutoff < 0)
00103 range_cutoff = scan_in.range_max;
00104
00105 unsigned int count = 0;
00106 for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
00107 {
00108 const float range = ranges(0, index);
00109 if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min)))
00110 {
00111 cloud_out.points[count].x = output(0,index);
00112 cloud_out.points[count].y = output(1,index);
00113 cloud_out.points[count].z = 0.0;
00114
00115
00116
00117
00118
00119
00120
00121
00122 if (idx_index != -1)
00123 cloud_out.channels[idx_index].values[count] = index;
00124
00125
00126 if (idx_distance != -1)
00127 cloud_out.channels[idx_distance].values[count] = range;
00128
00129
00130 if (scan_in.intensities.size() >= index)
00131 {
00132 if (idx_intensity != -1)
00133 cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
00134 }
00135
00136
00137 if( idx_timestamp != -1)
00138 cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
00139
00140 count++;
00141 }
00142 }
00143
00144
00145 cloud_out.points.resize (count);
00146 for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
00147 cloud_out.channels[d].values.resize(count);
00148 };
00149
00150 const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
00151 {
00152 boost::mutex::scoped_lock guv_lock(this->guv_mutex_);
00153
00154
00155 std::stringstream anglestring;
00156 anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
00157 std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
00158 it = unit_vector_map_.find(anglestring.str());
00159
00160 if (it != unit_vector_map_.end())
00161 return *((*it).second);
00162
00163 boost::numeric::ublas::matrix<double> * tempPtr = new boost::numeric::ublas::matrix<double>(2,length);
00164 for (unsigned int index = 0;index < length; index++)
00165 {
00166 (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
00167 (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
00168 }
00169
00170 unit_vector_map_[anglestring.str()] = tempPtr;
00171
00172 return *tempPtr;
00173 };
00174
00175
00176 LaserProjection::~LaserProjection()
00177 {
00178 std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
00179 it = unit_vector_map_.begin();
00180 while (it != unit_vector_map_.end())
00181 {
00182 delete (*it).second;
00183 it++;
00184 }
00185 };
00186
00187 void
00188 LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
00189 tf::Transformer& tf, double range_cutoff, int mask)
00190 {
00191 cloud_out.header = scan_in.header;
00192
00193 tf::Stamped<tf::Point> pointIn;
00194 tf::Stamped<tf::Point> pointOut;
00195
00196
00197 bool requested_index = false;
00198 if ((mask & channel_option::Index))
00199 requested_index = true;
00200
00201
00202
00203 mask |= channel_option::Index;
00204
00205 pointIn.frame_id_ = scan_in.header.frame_id;
00206
00207 projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask);
00208
00209 cloud_out.header.frame_id = target_frame;
00210
00211
00212 ros::Time start_time = scan_in.header.stamp ;
00213 ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment);
00214
00215 tf::StampedTransform start_transform ;
00216 tf::StampedTransform end_transform ;
00217 tf::StampedTransform cur_transform ;
00218
00219 tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
00220 tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
00221
00222
00223 int index_channel_idx = -1;
00224 for(unsigned int i = 0; i < cloud_out.channels.size(); ++i)
00225 {
00226 if(cloud_out.channels[i].name == "index")
00227 {
00228 index_channel_idx = i;
00229 break;
00230 }
00231 }
00232
00233
00234 ROS_ASSERT(index_channel_idx >= 0);
00235
00236 for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
00237 {
00238
00239 uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
00240
00241
00242 tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
00243
00245
00246
00247 tf::Vector3 v (0, 0, 0);
00248 v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
00249 cur_transform.setOrigin(v) ;
00250
00251
00252 tf::Quaternion q1, q2 ;
00253 start_transform.getBasis().getRotation(q1) ;
00254 end_transform.getBasis().getRotation(q2) ;
00255
00256
00257 cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
00258
00259
00260 tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
00261 tf::Vector3 pointOut = cur_transform * pointIn ;
00262
00263
00264 cloud_out.points[i].x = pointOut.x();
00265 cloud_out.points[i].y = pointOut.y();
00266 cloud_out.points[i].z = pointOut.z();
00267 }
00268
00269
00270 if(!requested_index)
00271 cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx);
00272 }
00273
00274 void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00275 sensor_msgs::PointCloud2 &cloud_out,
00276 double range_cutoff,
00277 int channel_options)
00278 {
00279 size_t n_pts = scan_in.ranges.size ();
00280 Eigen::ArrayXXd ranges (n_pts, 2);
00281 Eigen::ArrayXXd output (n_pts, 2);
00282
00283
00284 for (size_t i = 0; i < n_pts; ++i)
00285 {
00286 ranges (i, 0) = (double) scan_in.ranges[i];
00287 ranges (i, 1) = (double) scan_in.ranges[i];
00288 }
00289
00290
00291 if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max )
00292 {
00293 ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
00294 co_sine_map_ = Eigen::ArrayXXd (n_pts, 2);
00295 angle_min_ = scan_in.angle_min;
00296 angle_max_ = scan_in.angle_max;
00297
00298 for (size_t i = 0; i < n_pts; ++i)
00299 {
00300 co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment);
00301 co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
00302 }
00303 }
00304
00305 output = ranges * co_sine_map_;
00306
00307
00308 cloud_out.header = scan_in.header;
00309 cloud_out.height = 1;
00310 cloud_out.width = scan_in.ranges.size ();
00311 cloud_out.fields.resize (3);
00312 cloud_out.fields[0].name = "x";
00313 cloud_out.fields[0].offset = 0;
00314 cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00315 cloud_out.fields[0].count = 1;
00316 cloud_out.fields[1].name = "y";
00317 cloud_out.fields[1].offset = 4;
00318 cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00319 cloud_out.fields[1].count = 1;
00320 cloud_out.fields[2].name = "z";
00321 cloud_out.fields[2].offset = 8;
00322 cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00323 cloud_out.fields[2].count = 1;
00324
00325
00326 int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
00327
00328
00329 int offset = 12;
00330 if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
00331 {
00332 int field_size = cloud_out.fields.size();
00333 cloud_out.fields.resize(field_size + 1);
00334 cloud_out.fields[field_size].name = "intensity";
00335 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00336 cloud_out.fields[field_size].offset = offset;
00337 cloud_out.fields[field_size].count = 1;
00338 offset += 4;
00339 idx_intensity = field_size;
00340 }
00341
00342 if ((channel_options & channel_option::Index))
00343 {
00344 int field_size = cloud_out.fields.size();
00345 cloud_out.fields.resize(field_size + 1);
00346 cloud_out.fields[field_size].name = "index";
00347 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
00348 cloud_out.fields[field_size].offset = offset;
00349 cloud_out.fields[field_size].count = 1;
00350 offset += 4;
00351 idx_index = field_size;
00352 }
00353
00354 if ((channel_options & channel_option::Distance))
00355 {
00356 int field_size = cloud_out.fields.size();
00357 cloud_out.fields.resize(field_size + 1);
00358 cloud_out.fields[field_size].name = "distances";
00359 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00360 cloud_out.fields[field_size].offset = offset;
00361 cloud_out.fields[field_size].count = 1;
00362 offset += 4;
00363 idx_distance = field_size;
00364 }
00365
00366 if ((channel_options & channel_option::Timestamp))
00367 {
00368 int field_size = cloud_out.fields.size();
00369 cloud_out.fields.resize(field_size + 1);
00370 cloud_out.fields[field_size].name = "stamps";
00371 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00372 cloud_out.fields[field_size].offset = offset;
00373 cloud_out.fields[field_size].count = 1;
00374 offset += 4;
00375 idx_timestamp = field_size;
00376 }
00377
00378 if ((channel_options & channel_option::Viewpoint))
00379 {
00380 int field_size = cloud_out.fields.size();
00381 cloud_out.fields.resize(field_size + 3);
00382
00383 cloud_out.fields[field_size].name = "vp_x";
00384 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
00385 cloud_out.fields[field_size].offset = offset;
00386 cloud_out.fields[field_size].count = 1;
00387 offset += 4;
00388
00389 cloud_out.fields[field_size + 1].name = "vp_y";
00390 cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
00391 cloud_out.fields[field_size + 1].offset = offset;
00392 cloud_out.fields[field_size + 1].count = 1;
00393 offset += 4;
00394
00395 cloud_out.fields[field_size + 2].name = "vp_z";
00396 cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
00397 cloud_out.fields[field_size + 2].offset = offset;
00398 cloud_out.fields[field_size + 2].count = 1;
00399 offset += 4;
00400
00401 idx_vpx = field_size;
00402 idx_vpy = field_size + 1;
00403 idx_vpz = field_size + 2;
00404 }
00405
00406 cloud_out.point_step = offset;
00407 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
00408 cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
00409 cloud_out.is_dense = false;
00410
00411
00412
00413
00414 if (range_cutoff < 0)
00415 range_cutoff = scan_in.range_max;
00416
00417 unsigned int count = 0;
00418 for (size_t i = 0; i < n_pts; ++i)
00419 {
00420
00421 const float range = scan_in.ranges[i];
00422 if (range < range_cutoff && range >= scan_in.range_min)
00423 {
00424 float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
00425
00426
00427 pstep[0] = output (i, 0);
00428 pstep[1] = output (i, 1);
00429 pstep[2] = 0;
00430
00431
00432 if(idx_intensity != -1)
00433 pstep[idx_intensity] = scan_in.intensities[i];
00434
00435
00436 if(idx_index != -1)
00437 ((int*)(pstep))[idx_index] = i;
00438
00439
00440 if(idx_distance != -1)
00441 pstep[idx_distance] = range;
00442
00443
00444 if(idx_timestamp != -1)
00445 pstep[idx_timestamp] = i * scan_in.time_increment;
00446
00447
00448 if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
00449 {
00450 pstep[idx_vpx] = 0;
00451 pstep[idx_vpy] = 0;
00452 pstep[idx_vpz] = 0;
00453 }
00454
00455
00456 ++count;
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
00492 cloud_out.width = count;
00493 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
00494 cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
00495 }
00496
00497 void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
00498 const sensor_msgs::LaserScan &scan_in,
00499 sensor_msgs::PointCloud2 &cloud_out,
00500 tf::Transformer &tf,
00501 double range_cutoff,
00502 int channel_options)
00503 {
00504
00505 bool requested_index = false;
00506 if ((channel_options & channel_option::Index))
00507 requested_index = true;
00508
00509
00510
00511 channel_options |= channel_option::Index;
00512
00513 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00514
00515
00516 bool has_viewpoint = false;
00517 uint32_t vp_x_offset = 0;
00518
00519
00520
00521
00522
00523
00524 uint32_t index_offset = 0;
00525 for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
00526 {
00527 if(cloud_out.fields[i].name == "index")
00528 {
00529 index_offset = cloud_out.fields[i].offset;
00530 }
00531
00532
00533
00534
00535 if(cloud_out.fields[i].name == "vp_x")
00536 {
00537 has_viewpoint = true;
00538 vp_x_offset = cloud_out.fields[i].offset;
00539 }
00540 }
00541
00542 ROS_ASSERT(index_offset > 0);
00543
00544 cloud_out.header.frame_id = target_frame;
00545
00546
00547 ros::Time start_time = scan_in.header.stamp;
00548 ros::Time end_time = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
00549
00550 tf::StampedTransform start_transform, end_transform, cur_transform ;
00551
00552 tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
00553 tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
00554
00555 double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
00556
00557
00558 for(size_t i = 0; i < cloud_out.width; ++i)
00559 {
00560
00561 float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
00562
00563
00564 uint32_t pt_index;
00565 memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
00566
00567
00568 tfScalar ratio = pt_index * ranges_norm;
00569
00571
00572 tf::Vector3 v (0, 0, 0);
00573 v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
00574 cur_transform.setOrigin (v);
00575
00576
00577 tf::Quaternion q1, q2;
00578 start_transform.getBasis ().getRotation (q1);
00579 end_transform.getBasis ().getRotation (q2);
00580
00581
00582 cur_transform.setRotation (slerp (q1, q2 , ratio));
00583
00584 tf::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
00585 tf::Vector3 point_out = cur_transform * point_in;
00586
00587
00588 pstep[0] = point_out.x ();
00589 pstep[1] = point_out.y ();
00590 pstep[2] = point_out.z ();
00591
00592
00593 if(has_viewpoint)
00594 {
00595 float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
00596 point_in = tf::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
00597 point_out = cur_transform * point_in;
00598
00599
00600 vpstep[0] = point_out.x ();
00601 vpstep[1] = point_out.y ();
00602 vpstep[2] = point_out.z ();
00603 }
00604 }
00605
00606
00607 if(!requested_index)
00608 {
00609 sensor_msgs::PointCloud2 cloud_without_index;
00610
00611
00612 cloud_without_index.header = cloud_out.header;
00613 cloud_without_index.width = cloud_out.width;
00614 cloud_without_index.height = cloud_out.height;
00615 cloud_without_index.is_bigendian = cloud_out.is_bigendian;
00616 cloud_without_index.is_dense = cloud_out.is_dense;
00617
00618
00619 cloud_without_index.fields.resize(cloud_out.fields.size());
00620 unsigned int field_count = 0;
00621 unsigned int offset_shift = 0;
00622 for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
00623 {
00624 if(cloud_out.fields[i].name != "index")
00625 {
00626 cloud_without_index.fields[field_count] = cloud_out.fields[i];
00627 cloud_without_index.fields[field_count].offset -= offset_shift;
00628 ++field_count;
00629 }
00630 else
00631 {
00632
00633 offset_shift = 4;
00634 }
00635 }
00636
00637
00638 cloud_without_index.fields.resize(field_count);
00639
00640
00641 cloud_without_index.point_step = cloud_out.point_step - offset_shift;
00642 cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width;
00643 cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height);
00644
00645 uint32_t i = 0;
00646 uint32_t j = 0;
00647
00648 while (i < cloud_out.data.size())
00649 {
00650 if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4))
00651 {
00652 cloud_without_index.data[j++] = cloud_out.data[i];
00653 }
00654 i++;
00655 }
00656
00657
00658 cloud_out = cloud_without_index;
00659 }
00660 }
00661
00662
00663 }