projection_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 
32 #include "laser_geometry/laser_geometry.h"
33 #include "sensor_msgs/PointCloud.h"
34 #include "sensor_msgs/point_cloud2_iterator.h"
35 #include <math.h>
36 
37 
38 #include <angles/angles.h>
39 
40 #include "rostest/permuter.h"
41 
42 #define PROJECTION_TEST_RANGE_MIN (0.23)
43 #define PROJECTION_TEST_RANGE_MAX (40.0)
44 
45 
46 class BuildScanException { };
47 
48 sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
49  double ang_min, double ang_max, double ang_increment,
50  ros::Duration scan_time)
51 {
52  if (((ang_max - ang_min) / ang_increment) < 0)
53  throw (BuildScanException());
54 
55  sensor_msgs::LaserScan scan;
56  scan.header.stamp = ros::Time::now();
57  scan.header.frame_id = "laser_frame";
58  scan.angle_min = ang_min;
59  scan.angle_max = ang_max;
60  scan.angle_increment = ang_increment;
61  scan.scan_time = scan_time.toSec();
62  scan.range_min = PROJECTION_TEST_RANGE_MIN;
63  scan.range_max = PROJECTION_TEST_RANGE_MAX;
64  uint32_t i = 0;
65  for(; ang_min + i * ang_increment < ang_max; i++)
66  {
67  scan.ranges.push_back(range);
68  scan.intensities.push_back(intensity);
69  }
70 
71  scan.time_increment = scan_time.toSec()/(double)i;
72 
73  return scan;
74 };
75 
76 
77 class TestProjection : public laser_geometry::LaserProjection
78 {
79 public:
80  const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
81  double angle_max,
82  double angle_increment,
83  unsigned int length)
84  {
85  return getUnitVectors_(angle_min, angle_max, angle_increment, length);
86  }
87 };
88 
89 void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
90 {
91  double tolerance = 1e-12;
92  TestProjection projector;
93 
94  const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
95 
96 
97 
98  for (unsigned int i = 0; i < mat.size2(); i++)
99  {
100  EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
101  angle_min + i * angle_increment),
102  0,
103  tolerance); // check expected angle
104  EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
105  }
106 }
107 
108 #if 0
109 
110 TEST(laser_geometry, getUnitVectors)
111 {
112  double min_angle = -M_PI/2;
113  double max_angle = M_PI/2;
114  double angle_increment = M_PI/180;
115 
116  std::vector<double> min_angles, max_angles, angle_increments;
117 
118  rostest::Permuter permuter;
119  min_angles.push_back(-M_PI);
120  min_angles.push_back(-M_PI/1.5);
121  min_angles.push_back(-M_PI/2);
122  min_angles.push_back(-M_PI/4);
123  min_angles.push_back(-M_PI/8);
124  min_angles.push_back(M_PI);
125  min_angles.push_back(M_PI/1.5);
126  min_angles.push_back(M_PI/2);
127  min_angles.push_back(M_PI/4);
128  min_angles.push_back(M_PI/8);
129  permuter.addOptionSet(min_angles, &min_angle);
130 
131  max_angles.push_back(M_PI);
132  max_angles.push_back(M_PI/1.5);
133  max_angles.push_back(M_PI/2);
134  max_angles.push_back(M_PI/4);
135  max_angles.push_back(M_PI/8);
136  max_angles.push_back(-M_PI);
137  max_angles.push_back(-M_PI/1.5);
138  max_angles.push_back(-M_PI/2);
139  max_angles.push_back(-M_PI/4);
140  max_angles.push_back(-M_PI/8);
141  permuter.addOptionSet(max_angles, &max_angle);
142 
143  angle_increments.push_back(M_PI/180); // one degree
144  angle_increments.push_back(M_PI/360); // half degree
145  angle_increments.push_back(M_PI/720); // quarter degree
146  angle_increments.push_back(-M_PI/180); // -one degree
147  angle_increments.push_back(-M_PI/360); // -half degree
148  angle_increments.push_back(-M_PI/720); // -quarter degree
149  permuter.addOptionSet(angle_increments, &angle_increment);
150 
151 
152  while (permuter.step())
153  {
154  if ((max_angle - min_angle) / angle_increment > 0.0)
155  {
156  unsigned int length = round((max_angle - min_angle)/ angle_increment);
157  try
158  {
159  test_getUnitVectors(min_angle, max_angle, angle_increment, length);
160  test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
161  test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
162  }
163  catch (BuildScanException &ex)
164  {
165  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
166  FAIL();
167  }
168  }
169  //else
170  //printf("%f\n", (max_angle - min_angle) / angle_increment);
171  }
172 }
173 
174 
175 TEST(laser_geometry, projectLaser)
176 {
177  double tolerance = 1e-12;
178  laser_geometry::LaserProjection projector;
179 
180  double min_angle = -M_PI/2;
181  double max_angle = M_PI/2;
182  double angle_increment = M_PI/180;
183 
184  double range = 1.0;
185  double intensity = 1.0;
186 
187  ros::Duration scan_time = ros::Duration(1/40);
188  ros::Duration increment_time = ros::Duration(1/400);
189 
190 
191  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
192  std::vector<ros::Duration> increment_times, scan_times;
193 
194  rostest::Permuter permuter;
195 
196  ranges.push_back(-1.0);
197  ranges.push_back(1.0);
198  ranges.push_back(2.0);
199  ranges.push_back(3.0);
200  ranges.push_back(4.0);
201  ranges.push_back(5.0);
202  ranges.push_back(100.0);
203  permuter.addOptionSet(ranges, &range);
204 
205  intensities.push_back(1.0);
206  intensities.push_back(2.0);
207  intensities.push_back(3.0);
208  intensities.push_back(4.0);
209  intensities.push_back(5.0);
210  permuter.addOptionSet(intensities, &intensity);
211 
212  min_angles.push_back(-M_PI);
213  min_angles.push_back(-M_PI/1.5);
214  min_angles.push_back(-M_PI/2);
215  min_angles.push_back(-M_PI/4);
216  min_angles.push_back(-M_PI/8);
217  permuter.addOptionSet(min_angles, &min_angle);
218 
219  max_angles.push_back(M_PI);
220  max_angles.push_back(M_PI/1.5);
221  max_angles.push_back(M_PI/2);
222  max_angles.push_back(M_PI/4);
223  max_angles.push_back(M_PI/8);
224  permuter.addOptionSet(max_angles, &max_angle);
225 
226  // angle_increments.push_back(-M_PI/180); // -one degree
227  angle_increments.push_back(M_PI/180); // one degree
228  angle_increments.push_back(M_PI/360); // half degree
229  angle_increments.push_back(M_PI/720); // quarter degree
230  permuter.addOptionSet(angle_increments, &angle_increment);
231 
232  scan_times.push_back(ros::Duration(1/40));
233  scan_times.push_back(ros::Duration(1/20));
234 
235  permuter.addOptionSet(scan_times, &scan_time);
236 
237  while (permuter.step())
238  {
239  try
240  {
241  // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
242  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
243 
244  sensor_msgs::PointCloud cloud_out;
245  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
246  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
247  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
248  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
249 
250  projector.projectLaser(scan, cloud_out, -1.0);
251  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
252  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
253  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
254 
255  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
256  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
257 
258  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
259  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
260 
261  unsigned int valid_points = 0;
262  for (unsigned int i = 0; i < scan.ranges.size(); i++)
263  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
264  valid_points ++;
265 
266  EXPECT_EQ(valid_points, cloud_out.points.size());
267 
268  for (unsigned int i = 0; i < cloud_out.points.size(); i++)
269  {
270  EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
271  EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
272  EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
273  EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
274  EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
275  EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
276  EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
277  };
278  }
279  catch (BuildScanException &ex)
280  {
281  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
282  FAIL();
283  }
284  }
285 }
286 
287 #endif
288 
289 
290 TEST(laser_geometry, projectLaser2)
291 {
292  double tolerance = 1e-12;
293  laser_geometry::LaserProjection projector;
294 
295  double min_angle = -M_PI/2;
296  double max_angle = M_PI/2;
297  double angle_increment = M_PI/180;
298 
299  double range = 1.0;
300  double intensity = 1.0;
301 
302  ros::Duration scan_time = ros::Duration(1/40);
303  ros::Duration increment_time = ros::Duration(1/400);
304 
305 
306  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
307  std::vector<ros::Duration> increment_times, scan_times;
308 
309  rostest::Permuter permuter;
310 
311  ranges.push_back(-1.0);
312  ranges.push_back(1.0);
313  ranges.push_back(2.0);
314  ranges.push_back(3.0);
315  ranges.push_back(4.0);
316  ranges.push_back(5.0);
317  ranges.push_back(100.0);
318  permuter.addOptionSet(ranges, &range);
319 
320  intensities.push_back(1.0);
321  intensities.push_back(2.0);
322  intensities.push_back(3.0);
323  intensities.push_back(4.0);
324  intensities.push_back(5.0);
325  permuter.addOptionSet(intensities, &intensity);
326 
327  min_angles.push_back(-M_PI);
328  min_angles.push_back(-M_PI/1.5);
329  min_angles.push_back(-M_PI/2);
330  min_angles.push_back(-M_PI/4);
331  min_angles.push_back(-M_PI/8);
332  permuter.addOptionSet(min_angles, &min_angle);
333 
334  max_angles.push_back(M_PI);
335  max_angles.push_back(M_PI/1.5);
336  max_angles.push_back(M_PI/2);
337  max_angles.push_back(M_PI/4);
338  max_angles.push_back(M_PI/8);
339  permuter.addOptionSet(max_angles, &max_angle);
340 
341  // angle_increments.push_back(-M_PI/180); // -one degree
342  angle_increments.push_back(M_PI/180); // one degree
343  angle_increments.push_back(M_PI/360); // half degree
344  angle_increments.push_back(M_PI/720); // quarter degree
345  permuter.addOptionSet(angle_increments, &angle_increment);
346 
347  scan_times.push_back(ros::Duration(1/40));
348  scan_times.push_back(ros::Duration(1/20));
349 
350  permuter.addOptionSet(scan_times, &scan_time);
351 
352  while (permuter.step())
353  {
354  try
355  {
356  // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
357  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
358 
359  sensor_msgs::PointCloud2 cloud_out;
360  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
361  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
362  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
363  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
364 
365  projector.projectLaser(scan, cloud_out, -1.0);
366  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
367  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
368  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
369 
370  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
371  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
372 
373  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
374  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
375 
376  unsigned int valid_points = 0;
377  for (unsigned int i = 0; i < scan.ranges.size(); i++)
378  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
379  valid_points ++;
380 
381  EXPECT_EQ(valid_points, cloud_out.width);
382 
383  uint32_t x_offset = 0;
384  uint32_t y_offset = 0;
385  uint32_t z_offset = 0;
386  uint32_t intensity_offset = 0;
387  uint32_t index_offset = 0;
388  uint32_t distance_offset = 0;
389  uint32_t stamps_offset = 0;
390  for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
391  {
392  if (f->name == "x") x_offset = f->offset;
393  if (f->name == "y") y_offset = f->offset;
394  if (f->name == "z") z_offset = f->offset;
395  if (f->name == "intensity") intensity_offset = f->offset;
396  if (f->name == "index") index_offset = f->offset;
397  if (f->name == "distances") distance_offset = f->offset;
398  if (f->name == "stamps") stamps_offset = f->offset;
399  }
400 
401  for (unsigned int i = 0; i < cloud_out.width; i++)
402  {
403 
404  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
405  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
406  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
407  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
408  EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
409  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
410  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
411  };
412  }
413  catch (BuildScanException &ex)
414  {
415  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
416  FAIL();
417  }
418  }
419 }
420 
421 
422 TEST(laser_geometry, transformLaserScanToPointCloud)
423 {
424 
425  tf::Transformer tf;
426 
427  double tolerance = 1e-12;
428  laser_geometry::LaserProjection projector;
429 
430  double min_angle = -M_PI/2;
431  double max_angle = M_PI/2;
432  double angle_increment = M_PI/180;
433 
434  double range = 1.0;
435  double intensity = 1.0;
436 
437  ros::Duration scan_time = ros::Duration(1/40);
438  ros::Duration increment_time = ros::Duration(1/400);
439 
440 
441  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
442  std::vector<ros::Duration> increment_times, scan_times;
443 
444  rostest::Permuter permuter;
445 
446  ranges.push_back(-1.0);
447  ranges.push_back(1.0);
448  ranges.push_back(2.0);
449  ranges.push_back(3.0);
450  ranges.push_back(4.0);
451  ranges.push_back(5.0);
452  ranges.push_back(100.0);
453  permuter.addOptionSet(ranges, &range);
454 
455  intensities.push_back(1.0);
456  intensities.push_back(2.0);
457  intensities.push_back(3.0);
458  intensities.push_back(4.0);
459  intensities.push_back(5.0);
460  permuter.addOptionSet(intensities, &intensity);
461 
462  min_angles.push_back(-M_PI);
463  min_angles.push_back(-M_PI/1.5);
464  min_angles.push_back(-M_PI/2);
465  min_angles.push_back(-M_PI/4);
466  min_angles.push_back(-M_PI/8);
467 
468 
469  max_angles.push_back(M_PI);
470  max_angles.push_back(M_PI/1.5);
471  max_angles.push_back(M_PI/2);
472  max_angles.push_back(M_PI/4);
473  max_angles.push_back(M_PI/8);
474 
475  permuter.addOptionSet(min_angles, &min_angle);
476  permuter.addOptionSet(max_angles, &max_angle);
477 
478  angle_increments.push_back(-M_PI/180); // -one degree
479  angle_increments.push_back(M_PI/180); // one degree
480  angle_increments.push_back(M_PI/360); // half degree
481  angle_increments.push_back(M_PI/720); // quarter degree
482  permuter.addOptionSet(angle_increments, &angle_increment);
483 
484  scan_times.push_back(ros::Duration(1/40));
485  scan_times.push_back(ros::Duration(1/20));
486 
487  permuter.addOptionSet(scan_times, &scan_time);
488 
489  while (permuter.step())
490  {
491  try
492  {
493  //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
494  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
495  scan.header.frame_id = "laser_frame";
496 
497  sensor_msgs::PointCloud cloud_out;
498  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
499  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
500  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
501  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
502 
503  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
504  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
505  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
506  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
507 
508  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
509  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
510 
511  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
512  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
513 
514  unsigned int valid_points = 0;
515  for (unsigned int i = 0; i < scan.ranges.size(); i++)
516  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
517  valid_points ++;
518  EXPECT_EQ(valid_points, cloud_out.points.size());
519 
520  for (unsigned int i = 0; i < cloud_out.points.size(); i++)
521  {
522  EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
523  EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
524  EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
525  EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
526  EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
527  EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
528  EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
529  };
530  }
531  catch (BuildScanException &ex)
532  {
533  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
534  FAIL();
535  }
536  }
537 }
538 
539 TEST(laser_geometry, transformLaserScanToPointCloud2)
540 {
541 
542  tf::Transformer tf;
543  tf2::BufferCore tf2;
544 
545  double tolerance = 1e-12;
546  laser_geometry::LaserProjection projector;
547 
548  double min_angle = -M_PI/2;
549  double max_angle = M_PI/2;
550  double angle_increment = M_PI/180;
551 
552  double range = 1.0;
553  double intensity = 1.0;
554 
555  ros::Duration scan_time = ros::Duration(1/40);
556  ros::Duration increment_time = ros::Duration(1/400);
557 
558  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
559  std::vector<ros::Duration> increment_times, scan_times;
560 
561  rostest::Permuter permuter;
562 
563  ranges.push_back(-1.0);
564  ranges.push_back(1.0);
565  ranges.push_back(2.0);
566  ranges.push_back(3.0);
567  ranges.push_back(4.0);
568  ranges.push_back(5.0);
569  ranges.push_back(100.0);
570  permuter.addOptionSet(ranges, &range);
571 
572  intensities.push_back(1.0);
573  intensities.push_back(2.0);
574  intensities.push_back(3.0);
575  intensities.push_back(4.0);
576  intensities.push_back(5.0);
577  permuter.addOptionSet(intensities, &intensity);
578 
579  min_angles.push_back(-M_PI);
580  min_angles.push_back(-M_PI/1.5);
581  min_angles.push_back(-M_PI/2);
582  min_angles.push_back(-M_PI/4);
583  min_angles.push_back(-M_PI/8);
584 
585 
586  max_angles.push_back(M_PI);
587  max_angles.push_back(M_PI/1.5);
588  max_angles.push_back(M_PI/2);
589  max_angles.push_back(M_PI/4);
590  max_angles.push_back(M_PI/8);
591 
592  permuter.addOptionSet(min_angles, &min_angle);
593  permuter.addOptionSet(max_angles, &max_angle);
594 
595  angle_increments.push_back(-M_PI/180); // -one degree
596  angle_increments.push_back(M_PI/180); // one degree
597  angle_increments.push_back(M_PI/360); // half degree
598  angle_increments.push_back(M_PI/720); // quarter degree
599  permuter.addOptionSet(angle_increments, &angle_increment);
600 
601  scan_times.push_back(ros::Duration(1/40));
602  scan_times.push_back(ros::Duration(1/20));
603 
604  permuter.addOptionSet(scan_times, &scan_time);
605 
606  while (permuter.step())
607  {
608  try
609  {
610  //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
611  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
612  scan.header.frame_id = "laser_frame";
613 
614  sensor_msgs::PointCloud2 cloud_out;
615  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None);
616  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
617  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
618  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
619  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index);
620  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
621  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
622  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
623  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
624  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
625  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
626  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
627 
628  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
629  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
630  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
631  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
632  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
633  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
634  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
635  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
636 
637  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
638  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
639  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
640  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
641 
642  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
643  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
644  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
645  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
646 
647  EXPECT_EQ(cloud_out.is_dense, false);
648 
649  unsigned int valid_points = 0;
650  for (unsigned int i = 0; i < scan.ranges.size(); i++)
651  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
652  valid_points ++;
653  EXPECT_EQ(valid_points, cloud_out.width);
654 
655  uint32_t x_offset = 0;
656  uint32_t y_offset = 0;
657  uint32_t z_offset = 0;
658  uint32_t intensity_offset = 0;
659  uint32_t index_offset = 0;
660  uint32_t distance_offset = 0;
661  uint32_t stamps_offset = 0;
662  for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
663  {
664  if (f->name == "x") x_offset = f->offset;
665  if (f->name == "y") y_offset = f->offset;
666  if (f->name == "z") z_offset = f->offset;
667  if (f->name == "intensity") intensity_offset = f->offset;
668  if (f->name == "index") index_offset = f->offset;
669  if (f->name == "distances") distance_offset = f->offset;
670  if (f->name == "stamps") stamps_offset = f->offset;
671  }
672 
673  for (unsigned int i = 0; i < cloud_out.width; i++)
674  {
675  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
676  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
677  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
678  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
679  EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
680  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
681  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
682 
683  };
684  }
685  catch (BuildScanException &ex)
686  {
687  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
688  FAIL();
689  }
690  }
691 
692 }
693 
694 TEST(laser_geometry, transformLaserScanToPointCloudWithFixedFrame)
695 {
696  // The robot is driving forward 1 m/s starting at x=1 and rotating by pi/4 rad/s clockwise. The lidar starts scanning
697  // pointing 45 degrees left and makes 1/8 revolution clockwise per second. Every time it measures a point that is
698  // 1 m far. This creates an irregular pattern which tests that this function works correctly.
699 
700  /* L = lidar measurement, R = robot position; L2 and R3 coincide in the place of L2
701  | L2/R3 L3 |
702  | |
703  | R2 |
704  | |
705  | L1 R1 |
706  */
707 
708  tf2::BufferCore tf2;
709  geometry_msgs::TransformStamped t;
710  t.header.frame_id = "fixed";
711  t.child_frame_id = "laser";
712 
713  // drive straight 1 m/s, starting at x=1
714  t.header.stamp = ros::Time(10);
715  t.transform.translation.x = 1;
716  t.transform.rotation.z = -sin(M_PI/8);
717  t.transform.rotation.w = cos(M_PI/8);
718  tf2.setTransform(t, "test");
719 
720  t.header.stamp = ros::Time(11);
721  t.transform.translation.x = 2;
722  t.transform.rotation.z = 0;
723  t.transform.rotation.w = 1;
724  tf2.setTransform(t, "test");
725 
726  t.header.stamp = ros::Time(12);
727  t.transform.translation.x = 3;
728  t.transform.rotation.z = sin(M_PI/8);
729  t.transform.rotation.w = cos(M_PI/8);
730  tf2.setTransform(t, "test");
731 
732  laser_geometry::LaserProjection projector;
733  const auto tolerance = 1e-6;
734 
735  const auto min_angle = -M_PI/4;
736  const auto max_angle = M_PI/4;
737  const auto angle_increment = M_PI/4;
738 
739  const auto range = 1.0;
740  const auto intensity = 1.0;
741 
742  const ros::Duration scan_time {4};
743  const ros::Duration increment_time {1};
744 
745  sensor_msgs::LaserScan scan;
746  scan.header.frame_id = "laser";
747  scan.header.stamp = ros::Time(10);
748  scan.scan_time = scan_time.toSec();
749  scan.time_increment = increment_time.toSec();
750  scan.angle_increment = angle_increment;
751  scan.angle_min = min_angle;
752  scan.angle_max = max_angle;
753  scan.range_min = 0.1;
754  scan.range_max = 2.0;
755 
756  for (size_t i = 0; i < 3; ++i)
757  {
758  scan.ranges.push_back(range);
759  scan.intensities.push_back(intensity);
760  }
761 
762  sensor_msgs::PointCloud2 cloud_out;
763  projector.transformLaserScanToPointCloud("fixed", scan, cloud_out, "fixed", tf2, -1.0, laser_geometry::channel_option::Intensity);
764 
765  EXPECT_EQ(cloud_out.header.stamp, ros::Time(10));
766  EXPECT_EQ(cloud_out.header.frame_id, "fixed");
767 
768  ASSERT_EQ(cloud_out.width, 3);
769  ASSERT_EQ(cloud_out.fields.size(), 4);
770 
771  sensor_msgs::PointCloud2ConstIterator<float> x_it(cloud_out, "x");
772  sensor_msgs::PointCloud2ConstIterator<float> y_it(cloud_out, "y");
773  sensor_msgs::PointCloud2ConstIterator<float> z_it(cloud_out, "z");
774  sensor_msgs::PointCloud2ConstIterator<float> i_it(cloud_out, "intensity");
775  float x, y, z, i;
776 
777  x = *x_it; y = *y_it; z = *z_it; i = *i_it; ++x_it; ++y_it; ++z_it; ++i_it;
778  EXPECT_NEAR(x, 1, tolerance);
779  EXPECT_NEAR(y, -1, tolerance);
780  EXPECT_NEAR(z, 0, tolerance);
781  EXPECT_NEAR(i, 1, tolerance);
782 
783  x = *x_it; y = *y_it; z = *z_it; i = *i_it; ++x_it; ++y_it; ++z_it; ++i_it;
784  EXPECT_NEAR(x, 3, tolerance);
785  EXPECT_NEAR(y, 0, tolerance);
786  EXPECT_NEAR(z, 0, tolerance);
787  EXPECT_NEAR(i, intensity, tolerance);
788 
789  x = *x_it; y = *y_it; z = *z_it; i = *i_it; ++x_it; ++y_it; ++z_it; ++i_it;
790  EXPECT_NEAR(x, 3, tolerance);
791  EXPECT_NEAR(y, 1, tolerance);
792  EXPECT_NEAR(z, 0, tolerance);
793  EXPECT_NEAR(i, intensity, tolerance);
794 }
795 
796 
797 int main(int argc, char **argv){
798  ros::Time::init();
799  testing::InitGoogleTest(&argc, argv);
800  return RUN_ALL_TESTS();
801 }
laser_geometry::LaserProjection::projectLaser
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud.
Definition: laser_geometry.h:139
sensor_msgs::PointCloud2ConstIterator
laser_geometry::LaserProjection
A Class to Project Laser Scan.
Definition: laser_geometry.h:113
f
f
laser_geometry::channel_option::Index
Enable "index" channel.
Definition: laser_geometry.h:83
build_constant_scan
sensor_msgs::LaserScan build_constant_scan(double range, double intensity, double ang_min, double ang_max, double ang_increment, ros::Duration scan_time)
Definition: projection_test.cpp:48
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
TEST
TEST(laser_geometry, projectLaser2)
Definition: projection_test.cpp:290
PROJECTION_TEST_RANGE_MIN
#define PROJECTION_TEST_RANGE_MIN
Definition: projection_test.cpp:42
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
t
geometry_msgs::TransformStamped t
laser_geometry
Definition: laser_geometry.h:64
laser_geometry::channel_option::None
Enable no channels.
Definition: laser_geometry.h:81
PROJECTION_TEST_RANGE_MAX
#define PROJECTION_TEST_RANGE_MAX
Definition: projection_test.cpp:43
ros::Time::init
static void init()
laser_geometry::channel_option::Timestamp
Enable "stamps" channel.
Definition: laser_geometry.h:85
TestProjection::getUnitVectors
const boost::numeric::ublas::matrix< double > & getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
Definition: projection_test.cpp:80
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
test_getUnitVectors
void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
Definition: projection_test.cpp:89
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
laser_geometry::channel_option::Intensity
Enable "intensities" channel.
Definition: laser_geometry.h:82
laser_geometry.h
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
point_cloud2_iterator.h
ros::Time::now
static Time now()
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
laser_geometry::channel_option::Distance
Enable "distances" channel.
Definition: laser_geometry.h:84
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
laser_geometry::LaserProjection::transformLaserScanToPointCloud
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame.
Definition: laser_geometry.h:193
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
angles::shortest_angular_distance
def shortest_angular_distance(from_angle, to_angle)
main
int main(int argc, char **argv)
Definition: projection_test.cpp:797

laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu, William Woodall
autogenerated on Sun Feb 7 2021 03:09:58