laser_geometry: projection_test.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <gtest/gtest.h>
00031 
00032 #include "laser_geometry/laser_geometry.h"
00033 #include "sensor_msgs/PointCloud.h"
00034 #include <math.h>
00035 
00036 
00037 #include <angles/angles.h>
00038 
00039 #include "rostest/permuter.h"
00040 
00041 #define PROJECTION_TEST_RANGE_MIN (0.23)
00042 #define PROJECTION_TEST_RANGE_MAX (40.0) 
00043 
00044 
00045 class BuildScanException { };
00046 
00047 sensor_msgs::LaserScan build_constant_scan(double range, double intensity, 
00048                                           double ang_min, double ang_max, double ang_increment,
00049                                           ros::Duration scan_time)
00050 {
00051   if (((ang_max - ang_min) / ang_increment) < 0)
00052     throw (BuildScanException());
00053 
00054   sensor_msgs::LaserScan scan;
00055   scan.header.stamp = ros::Time::now();
00056   scan.header.frame_id = "laser_frame";
00057   scan.angle_min = ang_min;
00058   scan.angle_max = ang_max;
00059   scan.angle_increment = ang_increment;
00060   scan.scan_time = scan_time.toSec();
00061   scan.range_min = PROJECTION_TEST_RANGE_MIN;
00062   scan.range_max = PROJECTION_TEST_RANGE_MAX;
00063   uint32_t i = 0;
00064   for(; ang_min + i * ang_increment < ang_max; i++)
00065   {
00066     scan.ranges.push_back(range);
00067     scan.intensities.push_back(intensity);
00068   }
00069 
00070   scan.time_increment = scan_time.toSec()/(double)i;
00071 
00072   return scan;
00073 };
00074 
00075 
00076 class TestProjection : public laser_geometry::LaserProjection
00077 {
00078 public:
00079   const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
00080                                                               double angle_max,
00081                                                               double angle_increment,
00082                                                               unsigned int length)
00083   {
00084     return getUnitVectors_(angle_min, angle_max, angle_increment, length);
00085   }
00086 };
00087 
00088 void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
00089 {
00090   double tolerance = 1e-12;
00091   TestProjection projector;  
00092   
00093   const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
00094   
00095   
00096 
00097   for (unsigned int i = 0; i < mat.size2(); i++)
00098   {
00099     EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
00100                                                    angle_min + i * angle_increment),
00101                 0,
00102                 tolerance); // check expected angle
00103     EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
00104   }
00105 }
00106 
00107 #if 0
00108 
00109 TEST(laser_geometry, getUnitVectors)
00110 {
00111   double min_angle = -M_PI/2;
00112   double max_angle = M_PI/2;
00113   double angle_increment = M_PI/180;
00114 
00115   std::vector<double> min_angles, max_angles, angle_increments;
00116 
00117   rostest::Permuter permuter;
00118   min_angles.push_back(-M_PI);
00119   min_angles.push_back(-M_PI/1.5);
00120   min_angles.push_back(-M_PI/2);
00121   min_angles.push_back(-M_PI/4);
00122   min_angles.push_back(-M_PI/8);
00123   min_angles.push_back(M_PI);
00124   min_angles.push_back(M_PI/1.5);
00125   min_angles.push_back(M_PI/2);
00126   min_angles.push_back(M_PI/4);
00127   min_angles.push_back(M_PI/8);
00128   permuter.addOptionSet(min_angles, &min_angle);
00129 
00130   max_angles.push_back(M_PI);
00131   max_angles.push_back(M_PI/1.5);
00132   max_angles.push_back(M_PI/2);
00133   max_angles.push_back(M_PI/4);
00134   max_angles.push_back(M_PI/8);
00135   max_angles.push_back(-M_PI);
00136   max_angles.push_back(-M_PI/1.5);
00137   max_angles.push_back(-M_PI/2);
00138   max_angles.push_back(-M_PI/4);
00139   max_angles.push_back(-M_PI/8);
00140   permuter.addOptionSet(max_angles, &max_angle);
00141 
00142   angle_increments.push_back(M_PI/180); // one degree
00143   angle_increments.push_back(M_PI/360); // half degree
00144   angle_increments.push_back(M_PI/720); // quarter degree
00145   angle_increments.push_back(-M_PI/180); // -one degree
00146   angle_increments.push_back(-M_PI/360); // -half degree
00147   angle_increments.push_back(-M_PI/720); // -quarter degree
00148   permuter.addOptionSet(angle_increments, &angle_increment);
00149 
00150 
00151   while (permuter.step())
00152   {
00153     if ((max_angle - min_angle) / angle_increment > 0.0)
00154     {
00155       unsigned int length = round((max_angle - min_angle)/ angle_increment);
00156       try
00157       {
00158         test_getUnitVectors(min_angle, max_angle, angle_increment, length);
00159         test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
00160         test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
00161       }
00162       catch (BuildScanException &ex)
00163       {
00164       if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00165         FAIL();
00166       }
00167     }
00168     //else 
00169       //printf("%f\n", (max_angle - min_angle) / angle_increment);
00170   }
00171 }
00172 
00173 
00174 TEST(laser_geometry, projectLaser)
00175 {
00176   double tolerance = 1e-12;
00177   laser_geometry::LaserProjection projector;  
00178 
00179   double min_angle = -M_PI/2;
00180   double max_angle = M_PI/2;
00181   double angle_increment = M_PI/180;
00182 
00183   double range = 1.0;
00184   double intensity = 1.0;
00185 
00186   ros::Duration scan_time = ros::Duration(1/40);
00187   ros::Duration increment_time = ros::Duration(1/400);
00188 
00189 
00190   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00191   std::vector<ros::Duration> increment_times, scan_times;
00192 
00193   rostest::Permuter permuter;
00194 
00195   ranges.push_back(-1.0);
00196   ranges.push_back(1.0);
00197   ranges.push_back(2.0);
00198   ranges.push_back(3.0);
00199   ranges.push_back(4.0);
00200   ranges.push_back(5.0);
00201   ranges.push_back(100.0);
00202   permuter.addOptionSet(ranges, &range);
00203 
00204   intensities.push_back(1.0);
00205   intensities.push_back(2.0);
00206   intensities.push_back(3.0);
00207   intensities.push_back(4.0);
00208   intensities.push_back(5.0);
00209   permuter.addOptionSet(intensities, &intensity);
00210 
00211   min_angles.push_back(-M_PI);
00212   min_angles.push_back(-M_PI/1.5);
00213   min_angles.push_back(-M_PI/2);
00214   min_angles.push_back(-M_PI/4);
00215   min_angles.push_back(-M_PI/8);
00216   permuter.addOptionSet(min_angles, &min_angle);
00217 
00218   max_angles.push_back(M_PI);
00219   max_angles.push_back(M_PI/1.5);
00220   max_angles.push_back(M_PI/2);
00221   max_angles.push_back(M_PI/4);
00222   max_angles.push_back(M_PI/8);
00223   permuter.addOptionSet(max_angles, &max_angle);
00224 
00225   //  angle_increments.push_back(-M_PI/180); // -one degree
00226   angle_increments.push_back(M_PI/180); // one degree
00227   angle_increments.push_back(M_PI/360); // half degree
00228   angle_increments.push_back(M_PI/720); // quarter degree
00229   permuter.addOptionSet(angle_increments, &angle_increment);
00230 
00231   scan_times.push_back(ros::Duration(1/40));
00232   scan_times.push_back(ros::Duration(1/20));
00233 
00234   permuter.addOptionSet(scan_times, &scan_time);
00235 
00236   while (permuter.step())
00237   {
00238     try
00239     {
00240       //      printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00241   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00242 
00243   sensor_msgs::PointCloud cloud_out;
00244   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00245   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00246   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00247   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00248 
00249   projector.projectLaser(scan, cloud_out, -1.0);
00250   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00251   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00252   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00253 
00254   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00255   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00256 
00257   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);
00258   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00259 
00260   unsigned int valid_points = 0;
00261   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00262     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00263       valid_points ++;
00264 
00265   EXPECT_EQ(valid_points, cloud_out.points.size());
00266 
00267   for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00268   {
00269     EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00270     EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00271     EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00272     EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
00273     EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
00274     EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
00275     EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
00276   };
00277     }
00278     catch (BuildScanException &ex)
00279     {
00280         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00281             FAIL();
00282     }
00283   }
00284 }
00285 
00286 #endif
00287 
00288 
00289 TEST(laser_geometry, projectLaser2)
00290 {
00291   double tolerance = 1e-12;
00292   laser_geometry::LaserProjection projector;  
00293 
00294   double min_angle = -M_PI/2;
00295   double max_angle = M_PI/2;
00296   double angle_increment = M_PI/180;
00297 
00298   double range = 1.0;
00299   double intensity = 1.0;
00300 
00301   ros::Duration scan_time = ros::Duration(1/40);
00302   ros::Duration increment_time = ros::Duration(1/400);
00303 
00304 
00305   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00306   std::vector<ros::Duration> increment_times, scan_times;
00307 
00308   rostest::Permuter permuter;
00309 
00310   ranges.push_back(-1.0);
00311   ranges.push_back(1.0);
00312   ranges.push_back(2.0);
00313   ranges.push_back(3.0);
00314   ranges.push_back(4.0);
00315   ranges.push_back(5.0);
00316   ranges.push_back(100.0);
00317   permuter.addOptionSet(ranges, &range);
00318 
00319   intensities.push_back(1.0);
00320   intensities.push_back(2.0);
00321   intensities.push_back(3.0);
00322   intensities.push_back(4.0);
00323   intensities.push_back(5.0);
00324   permuter.addOptionSet(intensities, &intensity);
00325 
00326   min_angles.push_back(-M_PI);
00327   min_angles.push_back(-M_PI/1.5);
00328   min_angles.push_back(-M_PI/2);
00329   min_angles.push_back(-M_PI/4);
00330   min_angles.push_back(-M_PI/8);
00331   permuter.addOptionSet(min_angles, &min_angle);
00332 
00333   max_angles.push_back(M_PI);
00334   max_angles.push_back(M_PI/1.5);
00335   max_angles.push_back(M_PI/2);
00336   max_angles.push_back(M_PI/4);
00337   max_angles.push_back(M_PI/8);
00338   permuter.addOptionSet(max_angles, &max_angle);
00339 
00340   //  angle_increments.push_back(-M_PI/180); // -one degree
00341   angle_increments.push_back(M_PI/180); // one degree
00342   angle_increments.push_back(M_PI/360); // half degree
00343   angle_increments.push_back(M_PI/720); // quarter degree
00344   permuter.addOptionSet(angle_increments, &angle_increment);
00345 
00346   scan_times.push_back(ros::Duration(1/40));
00347   scan_times.push_back(ros::Duration(1/20));
00348 
00349   permuter.addOptionSet(scan_times, &scan_time);
00350 
00351   while (permuter.step())
00352   {
00353     try
00354     {
00355       //        printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00356   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00357 
00358   sensor_msgs::PointCloud2 cloud_out;
00359   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00360   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00361   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00362   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00363 
00364   projector.projectLaser(scan, cloud_out, -1.0);
00365   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00366   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00367   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00368 
00369   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00370   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00371 
00372   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);
00373   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00374 
00375   unsigned int valid_points = 0;
00376   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00377     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00378       valid_points ++;
00379 
00380   EXPECT_EQ(valid_points, cloud_out.width);
00381 
00382   uint32_t x_offset = 0;
00383   uint32_t y_offset = 0;
00384   uint32_t z_offset = 0;
00385   uint32_t intensity_offset = 0;
00386   uint32_t index_offset = 0;
00387   uint32_t distance_offset = 0;
00388   uint32_t stamps_offset = 0;
00389   for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00390   {
00391       if (f->name == "x") x_offset = f->offset;
00392       if (f->name == "y") y_offset = f->offset;
00393       if (f->name == "z") z_offset = f->offset;
00394       if (f->name == "intensity") intensity_offset = f->offset;
00395       if (f->name == "index") index_offset = f->offset;
00396       if (f->name == "distances") distance_offset = f->offset;
00397       if (f->name == "stamps") stamps_offset = f->offset;
00398   }
00399 
00400   for (unsigned int i = 0; i < cloud_out.width; i++)
00401   {
00402 
00403     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);
00404     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);
00405     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00406     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
00407     EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
00408     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
00409     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
00410   };
00411     }
00412     catch (BuildScanException &ex)
00413     {
00414         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00415             FAIL();
00416     }
00417   }
00418 }
00419 
00420 
00421 TEST(laser_geometry, transformLaserScanToPointCloud)
00422 {
00423 
00424   tf::Transformer tf;
00425   
00426   double tolerance = 1e-12;
00427   laser_geometry::LaserProjection projector;  
00428 
00429   double min_angle = -M_PI/2;
00430   double max_angle = M_PI/2;
00431   double angle_increment = M_PI/180;
00432 
00433   double range = 1.0;
00434   double intensity = 1.0;
00435 
00436   ros::Duration scan_time = ros::Duration(1/40);
00437   ros::Duration increment_time = ros::Duration(1/400);
00438 
00439 
00440   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00441   std::vector<ros::Duration> increment_times, scan_times;
00442 
00443   rostest::Permuter permuter;
00444 
00445   ranges.push_back(-1.0);
00446   ranges.push_back(1.0);
00447   ranges.push_back(2.0);
00448   ranges.push_back(3.0);
00449   ranges.push_back(4.0);
00450   ranges.push_back(5.0);
00451   ranges.push_back(100.0);
00452   permuter.addOptionSet(ranges, &range);
00453 
00454   intensities.push_back(1.0);
00455   intensities.push_back(2.0);
00456   intensities.push_back(3.0);
00457   intensities.push_back(4.0);
00458   intensities.push_back(5.0);
00459   permuter.addOptionSet(intensities, &intensity);
00460 
00461   min_angles.push_back(-M_PI);
00462   min_angles.push_back(-M_PI/1.5);
00463   min_angles.push_back(-M_PI/2);
00464   min_angles.push_back(-M_PI/4);
00465   min_angles.push_back(-M_PI/8);
00466   
00467 
00468   max_angles.push_back(M_PI);
00469   max_angles.push_back(M_PI/1.5);
00470   max_angles.push_back(M_PI/2);
00471   max_angles.push_back(M_PI/4);
00472   max_angles.push_back(M_PI/8);
00473 
00474   permuter.addOptionSet(min_angles, &min_angle);
00475   permuter.addOptionSet(max_angles, &max_angle);
00476 
00477   angle_increments.push_back(-M_PI/180); // -one degree
00478   angle_increments.push_back(M_PI/180); // one degree
00479   angle_increments.push_back(M_PI/360); // half degree
00480   angle_increments.push_back(M_PI/720); // quarter degree
00481   permuter.addOptionSet(angle_increments, &angle_increment);
00482 
00483   scan_times.push_back(ros::Duration(1/40));
00484   scan_times.push_back(ros::Duration(1/20));
00485 
00486   permuter.addOptionSet(scan_times, &scan_time);
00487 
00488   while (permuter.step())
00489   {
00490     try
00491     {    
00492     //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00493   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00494   scan.header.frame_id = "laser_frame";
00495 
00496   sensor_msgs::PointCloud cloud_out;
00497   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
00498   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00499   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
00500   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00501 
00502   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00503   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00504   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00505   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00506 
00507   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);
00508   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00509 
00510   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);
00511   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00512 
00513   unsigned int valid_points = 0;
00514   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00515     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00516       valid_points ++;    
00517   EXPECT_EQ(valid_points, cloud_out.points.size());
00518 
00519   for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00520   {
00521     EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00522     EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00523     EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00524     EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
00525     EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
00526     EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
00527     EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
00528   };
00529     }
00530     catch (BuildScanException &ex)
00531     {
00532         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00533             FAIL();
00534     }
00535   }
00536 }
00537 
00538 TEST(laser_geometry, transformLaserScanToPointCloud2)
00539 {
00540 
00541   tf::Transformer tf;
00542   tf2::BufferCore tf2;
00543 
00544   double tolerance = 1e-12;
00545   laser_geometry::LaserProjection projector;  
00546 
00547   double min_angle = -M_PI/2;
00548   double max_angle = M_PI/2;
00549   double angle_increment = M_PI/180;
00550 
00551   double range = 1.0;
00552   double intensity = 1.0;
00553 
00554   ros::Duration scan_time = ros::Duration(1/40);
00555   ros::Duration increment_time = ros::Duration(1/400);
00556 
00557   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00558   std::vector<ros::Duration> increment_times, scan_times;
00559 
00560   rostest::Permuter permuter;
00561 
00562   ranges.push_back(-1.0);
00563   ranges.push_back(1.0);
00564   ranges.push_back(2.0);
00565   ranges.push_back(3.0);
00566   ranges.push_back(4.0);
00567   ranges.push_back(5.0);
00568   ranges.push_back(100.0);
00569   permuter.addOptionSet(ranges, &range);
00570 
00571   intensities.push_back(1.0);
00572   intensities.push_back(2.0);
00573   intensities.push_back(3.0);
00574   intensities.push_back(4.0);
00575   intensities.push_back(5.0);
00576   permuter.addOptionSet(intensities, &intensity);
00577 
00578   min_angles.push_back(-M_PI);
00579   min_angles.push_back(-M_PI/1.5);
00580   min_angles.push_back(-M_PI/2);
00581   min_angles.push_back(-M_PI/4);
00582   min_angles.push_back(-M_PI/8);
00583   
00584 
00585   max_angles.push_back(M_PI);
00586   max_angles.push_back(M_PI/1.5);
00587   max_angles.push_back(M_PI/2);
00588   max_angles.push_back(M_PI/4);
00589   max_angles.push_back(M_PI/8);
00590 
00591   permuter.addOptionSet(min_angles, &min_angle);
00592   permuter.addOptionSet(max_angles, &max_angle);
00593 
00594   angle_increments.push_back(-M_PI/180); // -one degree
00595   angle_increments.push_back(M_PI/180); // one degree
00596   angle_increments.push_back(M_PI/360); // half degree
00597   angle_increments.push_back(M_PI/720); // quarter degree
00598   permuter.addOptionSet(angle_increments, &angle_increment);
00599 
00600   scan_times.push_back(ros::Duration(1/40));
00601   scan_times.push_back(ros::Duration(1/20));
00602 
00603   permuter.addOptionSet(scan_times, &scan_time);
00604 
00605   while (permuter.step())
00606   {
00607     try
00608     {    
00609     //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00610   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00611   scan.header.frame_id = "laser_frame";
00612 
00613   sensor_msgs::PointCloud2 cloud_out;
00614   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None);
00615   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
00616   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
00617   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
00618   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index);
00619   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00620   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
00621   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00622   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
00623   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00624   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
00625   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00626 
00627   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00628   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00629   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
00630   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00631   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00632   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00633   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00634   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00635 
00636   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);
00637   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00638   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);
00639   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00640 
00641   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);
00642   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00643   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);
00644   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00645 
00646   EXPECT_EQ(cloud_out.is_dense, false);
00647 
00648   unsigned int valid_points = 0;
00649   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00650     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00651       valid_points ++;    
00652   EXPECT_EQ(valid_points, cloud_out.width);
00653 
00654   uint32_t x_offset = 0;
00655   uint32_t y_offset = 0;
00656   uint32_t z_offset = 0;
00657   uint32_t intensity_offset = 0;
00658   uint32_t index_offset = 0;
00659   uint32_t distance_offset = 0;
00660   uint32_t stamps_offset = 0;
00661   for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00662   {
00663     if (f->name == "x") x_offset = f->offset;
00664     if (f->name == "y") y_offset = f->offset;
00665     if (f->name == "z") z_offset = f->offset;
00666     if (f->name == "intensity") intensity_offset = f->offset;
00667     if (f->name == "index") index_offset = f->offset;
00668     if (f->name == "distances") distance_offset = f->offset;
00669     if (f->name == "stamps") stamps_offset = f->offset;
00670   }
00671 
00672   for (unsigned int i = 0; i < cloud_out.width; i++)
00673   {
00674     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);
00675     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);
00676     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00677     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
00678     EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
00679     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
00680     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
00681 
00682   };
00683     }
00684     catch (BuildScanException &ex)
00685     {
00686         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00687             FAIL();
00688     }
00689   }
00690 
00691 }
00692 
00693 
00694 int main(int argc, char **argv){
00695   ros::Time::init();
00696   testing::InitGoogleTest(&argc, argv);
00697   return RUN_ALL_TESTS();
00698 }