41 bool preservative,
int mask)
43 boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
46 for (
unsigned int index = 0; index < scan_in.ranges.size(); index++)
48 ranges(0,index) = (double) scan_in.ranges[index];
49 ranges(1,index) = (double) scan_in.ranges[index];
54 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()));
57 cloud_out.header = scan_in.header;
58 cloud_out.points.resize (scan_in.ranges.size());
61 int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
63 cloud_out.channels.resize(0);
68 int chan_size = cloud_out.channels.size();
69 cloud_out.channels.resize (chan_size + 1);
70 cloud_out.channels[0].name =
"intensities";
71 cloud_out.channels[0].values.resize (scan_in.intensities.size());
78 int chan_size = cloud_out.channels.size();
79 cloud_out.channels.resize (chan_size +1);
80 cloud_out.channels[chan_size].name =
"index";
81 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
82 idx_index = chan_size;
88 int chan_size = cloud_out.channels.size();
89 cloud_out.channels.resize (chan_size + 1);
90 cloud_out.channels[chan_size].name =
"distances";
91 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
92 idx_distance = chan_size;
97 int chan_size = cloud_out.channels.size();
98 cloud_out.channels.resize (chan_size + 1);
99 cloud_out.channels[chan_size].name =
"stamps";
100 cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
101 idx_timestamp = chan_size;
104 if (range_cutoff < 0)
105 range_cutoff = scan_in.range_max;
107 unsigned int count = 0;
108 for (
unsigned int index = 0; index< scan_in.ranges.size(); index++)
110 const float range = ranges(0, index);
111 if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min)))
113 cloud_out.points[count].x = output(0,index);
114 cloud_out.points[count].y = output(1,index);
115 cloud_out.points[count].z = 0.0;
125 cloud_out.channels[idx_index].values[count] = index;
128 if (idx_distance != -1)
129 cloud_out.channels[idx_distance].values[count] = range;
132 if (scan_in.intensities.size() >= index)
134 if (idx_intensity != -1)
135 cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
139 if( idx_timestamp != -1)
140 cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
147 cloud_out.points.resize (count);
148 for (
unsigned int d = 0;
d < cloud_out.channels.size();
d++)
149 cloud_out.channels[
d].values.resize(count);
154 boost::mutex::scoped_lock guv_lock(this->
guv_mutex_);
157 std::stringstream anglestring;
158 anglestring <<angle_min<<
","<<angle_max<<
","<<angle_increment<<
","<<length;
159 std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
163 return *((*it).second);
165 boost::numeric::ublas::matrix<double> * tempPtr =
new boost::numeric::ublas::matrix<double>(2,length);
166 for (
unsigned int index = 0;index < length; index++)
168 (*tempPtr)(0,index) =
cos(angle_min + (
double) index * angle_increment);
169 (*tempPtr)(1,index) =
sin(angle_min + (
double) index * angle_increment);
180 std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
193 cloud_out.header = scan_in.header;
199 bool requested_index =
false;
200 if ((mask & channel_option::Index))
201 requested_index =
true;
207 pointIn.
frame_id_ = scan_in.header.frame_id;
209 projectLaser_ (scan_in, cloud_out, range_cutoff,
false, mask);
211 cloud_out.header.frame_id = target_frame;
214 ros::Time start_time = scan_in.header.stamp ;
215 ros::Time end_time = scan_in.header.stamp ;
216 if(!scan_in.ranges.empty()) end_time +=
ros::Duration().
fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment);
222 tf.
lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
223 tf.
lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
226 int index_channel_idx = -1;
227 for(
unsigned int i = 0; i < cloud_out.channels.size(); ++i)
229 if(cloud_out.channels[i].name ==
"index")
231 index_channel_idx = i;
239 for(
unsigned int i = 0; i < cloud_out.points.size(); ++i)
242 uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
245 tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
263 tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
267 cloud_out.points[i].
x = pointOut.
x();
268 cloud_out.points[i].y = pointOut.
y();
269 cloud_out.points[i].z = pointOut.
z();
274 cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx);
278 sensor_msgs::PointCloud2 &cloud_out,
282 size_t n_pts = scan_in.ranges.size ();
283 Eigen::ArrayXXd ranges (n_pts, 2);
284 Eigen::ArrayXXd output (n_pts, 2);
287 for (
size_t i = 0; i < n_pts; ++i)
289 ranges (i, 0) = (double) scan_in.ranges[i];
290 ranges (i, 1) = (double) scan_in.ranges[i];
296 ROS_DEBUG (
"[projectLaser] No precomputed map given. Computing one.");
301 for (
size_t i = 0; i < n_pts; ++i)
303 co_sine_map_ (i, 0) =
cos (scan_in.angle_min + (
double) i * scan_in.angle_increment);
304 co_sine_map_ (i, 1) =
sin (scan_in.angle_min + (
double) i * scan_in.angle_increment);
311 cloud_out.header = scan_in.header;
312 cloud_out.height = 1;
313 cloud_out.width = scan_in.ranges.size ();
314 cloud_out.fields.resize (3);
315 cloud_out.fields[0].name =
"x";
316 cloud_out.fields[0].offset = 0;
317 cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
318 cloud_out.fields[0].count = 1;
319 cloud_out.fields[1].name =
"y";
320 cloud_out.fields[1].offset = 4;
321 cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
322 cloud_out.fields[1].count = 1;
323 cloud_out.fields[2].name =
"z";
324 cloud_out.fields[2].offset = 8;
325 cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
326 cloud_out.fields[2].count = 1;
329 int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
333 if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
335 int field_size = cloud_out.fields.size();
336 cloud_out.fields.resize(field_size + 1);
337 cloud_out.fields[field_size].name =
"intensity";
338 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
339 cloud_out.fields[field_size].offset = offset;
340 cloud_out.fields[field_size].count = 1;
342 idx_intensity = field_size;
345 if ((channel_options & channel_option::Index))
347 int field_size = cloud_out.fields.size();
348 cloud_out.fields.resize(field_size + 1);
349 cloud_out.fields[field_size].name =
"index";
350 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
351 cloud_out.fields[field_size].offset = offset;
352 cloud_out.fields[field_size].count = 1;
354 idx_index = field_size;
357 if ((channel_options & channel_option::Distance))
359 int field_size = cloud_out.fields.size();
360 cloud_out.fields.resize(field_size + 1);
361 cloud_out.fields[field_size].name =
"distances";
362 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
363 cloud_out.fields[field_size].offset = offset;
364 cloud_out.fields[field_size].count = 1;
366 idx_distance = field_size;
369 if ((channel_options & channel_option::Timestamp))
371 int field_size = cloud_out.fields.size();
372 cloud_out.fields.resize(field_size + 1);
373 cloud_out.fields[field_size].name =
"stamps";
374 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
375 cloud_out.fields[field_size].offset = offset;
376 cloud_out.fields[field_size].count = 1;
378 idx_timestamp = field_size;
383 int field_size = cloud_out.fields.size();
384 cloud_out.fields.resize(field_size + 3);
386 cloud_out.fields[field_size].name =
"vp_x";
387 cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
388 cloud_out.fields[field_size].offset = offset;
389 cloud_out.fields[field_size].count = 1;
392 cloud_out.fields[field_size + 1].name =
"vp_y";
393 cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
394 cloud_out.fields[field_size + 1].offset = offset;
395 cloud_out.fields[field_size + 1].count = 1;
398 cloud_out.fields[field_size + 2].name =
"vp_z";
399 cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
400 cloud_out.fields[field_size + 2].offset = offset;
401 cloud_out.fields[field_size + 2].count = 1;
404 idx_vpx = field_size;
405 idx_vpy = field_size + 1;
406 idx_vpz = field_size + 2;
409 cloud_out.point_step = offset;
410 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
411 cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
412 cloud_out.is_dense =
false;
414 if (range_cutoff < 0)
415 range_cutoff = scan_in.range_max;
417 unsigned int count = 0;
418 for (
size_t i = 0; i < n_pts; ++i)
421 const float range = scan_in.ranges[i];
422 if (range < range_cutoff && range >= scan_in.range_min)
424 float *pstep = (
float*)&cloud_out.data[count * cloud_out.point_step];
427 pstep[0] = output (i, 0);
428 pstep[1] = output (i, 1);
432 if(idx_intensity != -1)
433 pstep[idx_intensity] = scan_in.intensities[i];
437 ((
int*)(pstep))[idx_index] = i;
440 if(idx_distance != -1)
441 pstep[idx_distance] = range;
444 if(idx_timestamp != -1)
445 pstep[idx_timestamp] = i * scan_in.time_increment;
448 if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
492 cloud_out.width = count;
493 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
494 cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
498 const sensor_msgs::LaserScan &scan_in,
499 sensor_msgs::PointCloud2 &cloud_out,
501 tf2::Vector3 origin_start,
503 tf2::Vector3 origin_end,
508 bool requested_index =
false;
509 if ((channel_options & channel_option::Index))
510 requested_index =
true;
516 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
519 bool has_viewpoint =
false;
520 uint32_t vp_x_offset = 0;
527 uint32_t index_offset = 0;
528 for(
unsigned int i = 0; i < cloud_out.fields.size(); ++i)
530 if(cloud_out.fields[i].name ==
"index")
532 index_offset = cloud_out.fields[i].offset;
538 if(cloud_out.fields[i].name ==
"vp_x")
540 has_viewpoint =
true;
541 vp_x_offset = cloud_out.fields[i].offset;
547 cloud_out.header.frame_id = target_frame;
551 double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
554 for(
size_t i = 0; i < cloud_out.width; ++i)
557 float *pstep = (
float*)&cloud_out.data[i * cloud_out.point_step + 0];
561 memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset],
sizeof(uint32_t));
564 tfScalar ratio = pt_index * ranges_norm;
568 tf2::Vector3 v (0, 0, 0);
569 v.setInterpolate3 (origin_start, origin_end, ratio);
575 tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
576 tf2::Vector3 point_out = cur_transform * point_in;
579 pstep[0] = point_out.x ();
580 pstep[1] = point_out.y ();
581 pstep[2] = point_out.z ();
586 float *vpstep = (
float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
587 point_in = tf2::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
588 point_out = cur_transform * point_in;
591 vpstep[0] = point_out.x ();
592 vpstep[1] = point_out.y ();
593 vpstep[2] = point_out.z ();
600 sensor_msgs::PointCloud2 cloud_without_index;
603 cloud_without_index.header = cloud_out.header;
604 cloud_without_index.width = cloud_out.width;
605 cloud_without_index.height = cloud_out.height;
606 cloud_without_index.is_bigendian = cloud_out.is_bigendian;
607 cloud_without_index.is_dense = cloud_out.is_dense;
610 cloud_without_index.fields.resize(cloud_out.fields.size());
611 unsigned int field_count = 0;
612 unsigned int offset_shift = 0;
613 for(
unsigned int i = 0; i < cloud_out.fields.size(); ++i)
615 if(cloud_out.fields[i].name !=
"index")
617 cloud_without_index.fields[field_count] = cloud_out.fields[i];
618 cloud_without_index.fields[field_count].offset -= offset_shift;
629 cloud_without_index.fields.resize(field_count);
632 cloud_without_index.point_step = cloud_out.point_step - offset_shift;
633 cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width;
634 cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height);
639 while (i < cloud_out.data.size())
641 if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4))
643 cloud_without_index.data[j++] = cloud_out.data[i];
649 cloud_out = cloud_without_index;
654 const sensor_msgs::LaserScan &scan_in,
655 sensor_msgs::PointCloud2 &cloud_out,
660 ros::Time start_time = scan_in.header.stamp;
661 ros::Time end_time = scan_in.header.stamp;
662 if(!scan_in.ranges.empty()) end_time +=
ros::Duration ().
fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
666 tf.
lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
667 tf.
lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
675 tf2::Vector3 origin_start(start_transform.
getOrigin().
getX(),
682 quat_start, origin_start,
683 quat_end, origin_end,
689 const sensor_msgs::LaserScan &scan_in,
690 sensor_msgs::PointCloud2 &cloud_out,
695 ros::Time start_time = scan_in.header.stamp;
696 ros::Time end_time = scan_in.header.stamp;
697 if(!scan_in.ranges.empty()) end_time +=
ros::Duration ().
fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
699 geometry_msgs::TransformStamped start_transform = tf.
lookupTransform (target_frame, scan_in.header.frame_id, start_time);
700 geometry_msgs::TransformStamped end_transform = tf.
lookupTransform (target_frame, scan_in.header.frame_id, end_time);
703 start_transform.transform.rotation.y,
704 start_transform.transform.rotation.z,
705 start_transform.transform.rotation.w);
707 end_transform.transform.rotation.y,
708 end_transform.transform.rotation.z,
709 end_transform.transform.rotation.w);
711 tf2::Vector3 origin_start(start_transform.transform.translation.x,
712 start_transform.transform.translation.y,
713 start_transform.transform.translation.z);
714 tf2::Vector3 origin_end(end_transform.transform.translation.x,
715 end_transform.transform.translation.y,
716 end_transform.transform.translation.z);
718 quat_start, origin_start,
719 quat_end, origin_end,
725 const sensor_msgs::LaserScan &scan_in,
726 sensor_msgs::PointCloud2 &cloud_out,
727 const std::string &fixed_frame,
732 const ros::Time& start_time = scan_in.header.stamp;
733 ros::Time end_time = scan_in.header.stamp;
734 if(!scan_in.ranges.empty()) end_time +=
ros::Duration ().
fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
736 const geometry_msgs::TransformStamped start_transform = tf.
lookupTransform (target_frame, scan_in.header.frame_id, start_time);
737 const geometry_msgs::TransformStamped end_transform = tf.
lookupTransform (target_frame, start_time, scan_in.header.frame_id, end_time, fixed_frame);
739 const geometry_msgs::Transform& start_tf = start_transform.transform;
740 const geometry_msgs::Transform& end_tf = end_transform.transform;
745 tf2::Vector3 origin_start;
tf2::convert(start_tf.translation, origin_start);
746 tf2::Vector3 origin_end;
tf2::convert(end_tf.translation, origin_end);
749 quat_start, origin_start,
750 quat_end, origin_end,
751 range_cutoff, channel_options);
void projectLaser_(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff, bool preservative, int channel_options)
Internal hidden representation of projectLaser.
std::map< std::string, boost::numeric::ublas::matrix< double > * > unit_vector_map_
Internal map of pointers to stored values.
TFSIMD_FORCE_INLINE const tfScalar & getY() const
TFSIMD_FORCE_INLINE const tfScalar & getW() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Eigen::ArrayXXd co_sine_map_
void getRotation(Quaternion &q) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE void setInterpolate3(const Vector3 &v0, const Vector3 &v1, tfScalar rt)
~LaserProjection()
Destructor to deallocate stored unit vectors.
Duration & fromSec(double t)
TFSIMD_FORCE_INLINE const tfScalar & y() const
const boost::numeric::ublas::matrix< double > & getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
Internal protected representation of getUnitVectors.
void transformLaserScanToPointCloud_(const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, double range_cutoff, int channel_options)
Internal hidden representation of transformLaserScanToPointCloud.
Enable "viewpoint" channel.
Enable "intensities" channel.
TFSIMD_FORCE_INLINE const tfScalar & getX() const
void convert(const A &a, B &b)
Enable "distances" channel.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)