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
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 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)))
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
00119
00120
00121
00122
00123
00124
00125 if (idx_index != -1)
00126 cloud_out.channels[idx_index].values[count] = index;
00127
00128
00129 if (idx_distance != -1)
00130 cloud_out.channels[idx_distance].values[count] = ranges (0, index);
00131
00132
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
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
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
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
00164 if (it != unit_vector_map_.end())
00165 return *((*it).second);
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
00174 unit_vector_map_[anglestring.str()] = tempPtr;
00175
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
00201 bool requested_index = false;
00202 if ((mask & channel_option::Index))
00203 requested_index = true;
00204
00205
00206
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
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
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
00238 ROS_ASSERT(index_channel_idx >= 0);
00239
00240 for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
00241 {
00242
00243 uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
00244
00245
00246 tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
00247
00249
00250
00251 tf::Vector3 v (0, 0, 0);
00252 v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
00253 cur_transform.setOrigin(v) ;
00254
00255
00256 tf::Quaternion q1, q2 ;
00257 start_transform.getBasis().getRotation(q1) ;
00258 end_transform.getBasis().getRotation(q2) ;
00259
00260
00261 cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
00262
00263
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
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
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
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
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
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
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
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
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
00416
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
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
00432 pstep[0] = output (i, 0);
00433 pstep[1] = output (i, 1);
00434 pstep[2] = 0;
00435
00436
00437 if(idx_intensity != -1)
00438 pstep[idx_intensity] = scan_in.intensities[i];
00439
00440
00441 if(idx_index != -1)
00442 ((int*)(pstep))[idx_index] = i;
00443
00444
00445 if(idx_distance != -1)
00446 pstep[idx_distance] = scan_in.ranges[i];
00447
00448
00449 if(idx_timestamp != -1)
00450 pstep[idx_timestamp] = i * scan_in.time_increment;
00451
00452
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
00461 ++count;
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
00493
00494 }
00495
00496
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
00510 bool requested_index = false;
00511 if ((channel_options & channel_option::Index))
00512 requested_index = true;
00513
00514
00515
00516 channel_options |= channel_option::Index;
00517
00518 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00519
00520
00521 bool has_viewpoint = false;
00522 uint32_t vp_x_offset = 0;
00523
00524
00525
00526
00527
00528
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
00538
00539
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
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
00563 for(size_t i = 0; i < cloud_out.width; ++i)
00564 {
00565
00566 float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
00567
00568
00569 uint32_t pt_index;
00570 memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
00571
00572
00573 tfScalar ratio = pt_index * ranges_norm;
00574
00576
00577 tf::Vector3 v (0, 0, 0);
00578 v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
00579 cur_transform.setOrigin (v);
00580
00581
00582 tf::Quaternion q1, q2;
00583 start_transform.getBasis ().getRotation (q1);
00584 end_transform.getBasis ().getRotation (q2);
00585
00586
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
00593 pstep[0] = point_out.x ();
00594 pstep[1] = point_out.y ();
00595 pstep[2] = point_out.z ();
00596
00597
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
00605 vpstep[0] = point_out.x ();
00606 vpstep[1] = point_out.y ();
00607 vpstep[2] = point_out.z ();
00608 }
00609 }
00610
00611
00612 if(!requested_index)
00613 {
00614 sensor_msgs::PointCloud2 cloud_without_index;
00615
00616
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
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
00638 offset_shift = 4;
00639 }
00640 }
00641
00642
00643 cloud_without_index.fields.resize(field_count);
00644
00645
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
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
00663 cloud_out = cloud_without_index;
00664 }
00665 }
00666
00667
00668 }