laser_geometry: laser_geometry: projection

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 #include <sys/time.h>
00032 
00033 #include "laser_geometry/laser_geometry.h"
00034 #include "sensor_msgs/PointCloud.h"
00035 #include <math.h>
00036 
00037 
00038 #include "angles/angles.h"
00039 
00040 #include "rostest/permuter.h"
00041 
00042 #define PROJECTION_TEST_RANGE_MIN (0.23)
00043 #define PROJECTION_TEST_RANGE_MAX (40.0) 
00044 
00045 
00046 class BuildScanException { };
00047 
00048 sensor_msgs::LaserScan build_constant_scan(double range, double intensity, 
00049                                           double ang_min, double ang_max, double ang_increment,
00050                                           ros::Duration scan_time)
00051 {
00052   if (((ang_max - ang_min) / ang_increment) < 0)
00053     throw (BuildScanException());
00054 
00055   sensor_msgs::LaserScan scan;
00056   scan.header.stamp = ros::Time::now();
00057   scan.header.frame_id = "laser_frame";
00058   scan.angle_min = ang_min;
00059   scan.angle_max = ang_max;
00060   scan.angle_increment = ang_increment;
00061   scan.scan_time = scan_time.toSec();
00062   scan.range_min = PROJECTION_TEST_RANGE_MIN;
00063   scan.range_max = PROJECTION_TEST_RANGE_MAX;
00064   uint32_t i = 0;
00065   for(; ang_min + i * ang_increment < ang_max; i++)
00066   {
00067     scan.ranges.push_back(range);
00068     scan.intensities.push_back(intensity);
00069   }
00070 
00071   scan.time_increment = scan_time.toSec()/(double)i;
00072 
00073   return scan;
00074 };
00075 
00076 
00077 class TestProjection : public laser_geometry::LaserProjection
00078 {
00079 public:
00080   const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
00081                                                               double angle_max,
00082                                                               double angle_increment,
00083                                                               unsigned int length)
00084   {
00085     return getUnitVectors_(angle_min, angle_max, angle_increment, length);
00086   }
00087 };
00088 
00089 void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
00090 {
00091   double tolerance = 1e-12;
00092   TestProjection projector;  
00093   
00094   const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
00095   
00096   
00097 
00098   for (unsigned int i = 0; i < mat.size2(); i++)
00099   {
00100     EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
00101                                                    angle_min + i * angle_increment),
00102                 0,
00103                 tolerance); // check expected angle
00104     EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
00105   }
00106 }
00107 
00108 #if 0
00109 
00110 TEST(laser_geometry, getUnitVectors)
00111 {
00112   double min_angle = -M_PI/2;
00113   double max_angle = M_PI/2;
00114   double angle_increment = M_PI/180;
00115 
00116   std::vector<double> min_angles, max_angles, angle_increments;
00117 
00118   rostest::Permuter permuter;
00119   min_angles.push_back(-M_PI);
00120   min_angles.push_back(-M_PI/1.5);
00121   min_angles.push_back(-M_PI/2);
00122   min_angles.push_back(-M_PI/4);
00123   min_angles.push_back(-M_PI/8);
00124   min_angles.push_back(M_PI);
00125   min_angles.push_back(M_PI/1.5);
00126   min_angles.push_back(M_PI/2);
00127   min_angles.push_back(M_PI/4);
00128   min_angles.push_back(M_PI/8);
00129   permuter.addOptionSet(min_angles, &min_angle);
00130 
00131   max_angles.push_back(M_PI);
00132   max_angles.push_back(M_PI/1.5);
00133   max_angles.push_back(M_PI/2);
00134   max_angles.push_back(M_PI/4);
00135   max_angles.push_back(M_PI/8);
00136   max_angles.push_back(-M_PI);
00137   max_angles.push_back(-M_PI/1.5);
00138   max_angles.push_back(-M_PI/2);
00139   max_angles.push_back(-M_PI/4);
00140   max_angles.push_back(-M_PI/8);
00141   permuter.addOptionSet(max_angles, &max_angle);
00142 
00143   angle_increments.push_back(M_PI/180); // one degree
00144   angle_increments.push_back(M_PI/360); // half degree
00145   angle_increments.push_back(M_PI/720); // quarter degree
00146   angle_increments.push_back(-M_PI/180); // -one degree
00147   angle_increments.push_back(-M_PI/360); // -half degree
00148   angle_increments.push_back(-M_PI/720); // -quarter degree
00149   permuter.addOptionSet(angle_increments, &angle_increment);
00150 
00151 
00152   while (permuter.step())
00153   {
00154     if ((max_angle - min_angle) / angle_increment > 0.0)
00155     {
00156       unsigned int length = round((max_angle - min_angle)/ angle_increment);
00157       try
00158       {
00159         test_getUnitVectors(min_angle, max_angle, angle_increment, length);
00160         test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
00161         test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
00162       }
00163       catch (BuildScanException &ex)
00164       {
00165       if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00166         FAIL();
00167       }
00168     }
00169     //else 
00170       //printf("%f\n", (max_angle - min_angle) / angle_increment);
00171   }
00172 }
00173 
00174 
00175 TEST(laser_geometry, projectLaser)
00176 {
00177   double tolerance = 1e-12;
00178   laser_geometry::LaserProjection projector;  
00179 
00180   double min_angle = -M_PI/2;
00181   double max_angle = M_PI/2;
00182   double angle_increment = M_PI/180;
00183 
00184   double range = 1.0;
00185   double intensity = 1.0;
00186 
00187   ros::Duration scan_time = ros::Duration(1/40);
00188   ros::Duration increment_time = ros::Duration(1/400);
00189 
00190 
00191   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00192   std::vector<ros::Duration> increment_times, scan_times;
00193 
00194   rostest::Permuter permuter;
00195 
00196   ranges.push_back(-1.0);
00197   ranges.push_back(1.0);
00198   ranges.push_back(2.0);
00199   ranges.push_back(3.0);
00200   ranges.push_back(4.0);
00201   ranges.push_back(5.0);
00202   ranges.push_back(100.0);
00203   permuter.addOptionSet(ranges, &range);
00204 
00205   intensities.push_back(1.0);
00206   intensities.push_back(2.0);
00207   intensities.push_back(3.0);
00208   intensities.push_back(4.0);
00209   intensities.push_back(5.0);
00210   permuter.addOptionSet(intensities, &intensity);
00211 
00212   min_angles.push_back(-M_PI);
00213   min_angles.push_back(-M_PI/1.5);
00214   min_angles.push_back(-M_PI/2);
00215   min_angles.push_back(-M_PI/4);
00216   min_angles.push_back(-M_PI/8);
00217   permuter.addOptionSet(min_angles, &min_angle);
00218 
00219   max_angles.push_back(M_PI);
00220   max_angles.push_back(M_PI/1.5);
00221   max_angles.push_back(M_PI/2);
00222   max_angles.push_back(M_PI/4);
00223   max_angles.push_back(M_PI/8);
00224   permuter.addOptionSet(max_angles, &max_angle);
00225 
00226   //  angle_increments.push_back(-M_PI/180); // -one degree
00227   angle_increments.push_back(M_PI/180); // one degree
00228   angle_increments.push_back(M_PI/360); // half degree
00229   angle_increments.push_back(M_PI/720); // quarter degree
00230   permuter.addOptionSet(angle_increments, &angle_increment);
00231 
00232   scan_times.push_back(ros::Duration(1/40));
00233   scan_times.push_back(ros::Duration(1/20));
00234 
00235   permuter.addOptionSet(scan_times, &scan_time);
00236 
00237   while (permuter.step())
00238   {
00239     try
00240     {
00241       //      printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00242   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00243 
00244   sensor_msgs::PointCloud cloud_out;
00245   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00246   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00247   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00248   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00249 
00250   projector.projectLaser(scan, cloud_out, -1.0);
00251   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00252   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00253   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00254 
00255   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00256   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00257 
00258   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);
00259   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00260 
00261   unsigned int valid_points = 0;
00262   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00263     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00264       valid_points ++;
00265 
00266   EXPECT_EQ(valid_points, cloud_out.points.size());
00267 
00268   for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00269   {
00270     EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00271     EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00272     EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00273     EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
00274     EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
00275     EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
00276     EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
00277   };
00278     }
00279     catch (BuildScanException &ex)
00280     {
00281         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00282             FAIL();
00283     }
00284   }
00285 }
00286 
00287 #endif
00288 
00289 
00290 TEST(laser_geometry, projectLaser2)
00291 {
00292   double tolerance = 1e-12;
00293   laser_geometry::LaserProjection projector;  
00294 
00295   double min_angle = -M_PI/2;
00296   double max_angle = M_PI/2;
00297   double angle_increment = M_PI/180;
00298 
00299   double range = 1.0;
00300   double intensity = 1.0;
00301 
00302   ros::Duration scan_time = ros::Duration(1/40);
00303   ros::Duration increment_time = ros::Duration(1/400);
00304 
00305 
00306   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00307   std::vector<ros::Duration> increment_times, scan_times;
00308 
00309   rostest::Permuter permuter;
00310 
00311   ranges.push_back(-1.0);
00312   ranges.push_back(1.0);
00313   ranges.push_back(2.0);
00314   ranges.push_back(3.0);
00315   ranges.push_back(4.0);
00316   ranges.push_back(5.0);
00317   ranges.push_back(100.0);
00318   permuter.addOptionSet(ranges, &range);
00319 
00320   intensities.push_back(1.0);
00321   intensities.push_back(2.0);
00322   intensities.push_back(3.0);
00323   intensities.push_back(4.0);
00324   intensities.push_back(5.0);
00325   permuter.addOptionSet(intensities, &intensity);
00326 
00327   min_angles.push_back(-M_PI);
00328   min_angles.push_back(-M_PI/1.5);
00329   min_angles.push_back(-M_PI/2);
00330   min_angles.push_back(-M_PI/4);
00331   min_angles.push_back(-M_PI/8);
00332   permuter.addOptionSet(min_angles, &min_angle);
00333 
00334   max_angles.push_back(M_PI);
00335   max_angles.push_back(M_PI/1.5);
00336   max_angles.push_back(M_PI/2);
00337   max_angles.push_back(M_PI/4);
00338   max_angles.push_back(M_PI/8);
00339   permuter.addOptionSet(max_angles, &max_angle);
00340 
00341   //  angle_increments.push_back(-M_PI/180); // -one degree
00342   angle_increments.push_back(M_PI/180); // one degree
00343   angle_increments.push_back(M_PI/360); // half degree
00344   angle_increments.push_back(M_PI/720); // quarter degree
00345   permuter.addOptionSet(angle_increments, &angle_increment);
00346 
00347   scan_times.push_back(ros::Duration(1/40));
00348   scan_times.push_back(ros::Duration(1/20));
00349 
00350   permuter.addOptionSet(scan_times, &scan_time);
00351 
00352   while (permuter.step())
00353   {
00354     try
00355     {
00356       //        printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00357   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00358 
00359   sensor_msgs::PointCloud2 cloud_out;
00360   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00361   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00362   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00363   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00364 
00365   projector.projectLaser(scan, cloud_out, -1.0);
00366   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00367   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00368   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00369 
00370   projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00371   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00372 
00373   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);
00374   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00375 
00376   unsigned int valid_points = 0;
00377   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00378     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00379       valid_points ++;
00380 
00381   EXPECT_EQ(valid_points, cloud_out.width);
00382 
00383   uint32_t x_offset = 0;
00384   uint32_t y_offset = 0;
00385   uint32_t z_offset = 0;
00386   uint32_t intensity_offset = 0;
00387   uint32_t index_offset = 0;
00388   uint32_t distance_offset = 0;
00389   uint32_t stamps_offset = 0;
00390   for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00391   {
00392       if (f->name == "x") x_offset = f->offset;
00393       if (f->name == "y") y_offset = f->offset;
00394       if (f->name == "z") z_offset = f->offset;
00395       if (f->name == "intensity") intensity_offset = f->offset;
00396       if (f->name == "index") index_offset = f->offset;
00397       if (f->name == "distances") distance_offset = f->offset;
00398       if (f->name == "stamps") stamps_offset = f->offset;
00399   }
00400 
00401   for (unsigned int i = 0; i < cloud_out.width; i++)
00402   {
00403 
00404     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);
00405     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);
00406     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00407     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
00408     EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
00409     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
00410     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
00411   };
00412     }
00413     catch (BuildScanException &ex)
00414     {
00415         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00416             FAIL();
00417     }
00418   }
00419 }
00420 
00421 
00422 TEST(laser_geometry, transformLaserScanToPointCloud)
00423 {
00424 
00425   tf::Transformer tf;
00426   
00427   double tolerance = 1e-12;
00428   laser_geometry::LaserProjection projector;  
00429 
00430   double min_angle = -M_PI/2;
00431   double max_angle = M_PI/2;
00432   double angle_increment = M_PI/180;
00433 
00434   double range = 1.0;
00435   double intensity = 1.0;
00436 
00437   ros::Duration scan_time = ros::Duration(1/40);
00438   ros::Duration increment_time = ros::Duration(1/400);
00439 
00440 
00441   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00442   std::vector<ros::Duration> increment_times, scan_times;
00443 
00444   rostest::Permuter permuter;
00445 
00446   ranges.push_back(-1.0);
00447   ranges.push_back(1.0);
00448   ranges.push_back(2.0);
00449   ranges.push_back(3.0);
00450   ranges.push_back(4.0);
00451   ranges.push_back(5.0);
00452   ranges.push_back(100.0);
00453   permuter.addOptionSet(ranges, &range);
00454 
00455   intensities.push_back(1.0);
00456   intensities.push_back(2.0);
00457   intensities.push_back(3.0);
00458   intensities.push_back(4.0);
00459   intensities.push_back(5.0);
00460   permuter.addOptionSet(intensities, &intensity);
00461 
00462   min_angles.push_back(-M_PI);
00463   min_angles.push_back(-M_PI/1.5);
00464   min_angles.push_back(-M_PI/2);
00465   min_angles.push_back(-M_PI/4);
00466   min_angles.push_back(-M_PI/8);
00467   
00468 
00469   max_angles.push_back(M_PI);
00470   max_angles.push_back(M_PI/1.5);
00471   max_angles.push_back(M_PI/2);
00472   max_angles.push_back(M_PI/4);
00473   max_angles.push_back(M_PI/8);
00474 
00475   permuter.addOptionSet(min_angles, &min_angle);
00476   permuter.addOptionSet(max_angles, &max_angle);
00477 
00478   angle_increments.push_back(-M_PI/180); // -one degree
00479   angle_increments.push_back(M_PI/180); // one degree
00480   angle_increments.push_back(M_PI/360); // half degree
00481   angle_increments.push_back(M_PI/720); // quarter degree
00482   permuter.addOptionSet(angle_increments, &angle_increment);
00483 
00484   scan_times.push_back(ros::Duration(1/40));
00485   scan_times.push_back(ros::Duration(1/20));
00486 
00487   permuter.addOptionSet(scan_times, &scan_time);
00488 
00489   while (permuter.step())
00490   {
00491     try
00492     {    
00493     //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
00494   sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00495   scan.header.frame_id = "laser_frame";
00496 
00497   sensor_msgs::PointCloud cloud_out;
00498   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
00499   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00500   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
00501   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00502 
00503   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00504   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00505   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00506   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00507 
00508   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);
00509   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00510 
00511   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);
00512   EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00513 
00514   unsigned int valid_points = 0;
00515   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00516     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00517       valid_points ++;    
00518   EXPECT_EQ(valid_points, cloud_out.points.size());
00519 
00520   for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00521   {
00522     EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00523     EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00524     EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00525     EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
00526     EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
00527     EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
00528     EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
00529   };
00530     }
00531     catch (BuildScanException &ex)
00532     {
00533         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00534             FAIL();
00535     }
00536   }
00537 }
00538 
00539 TEST(laser_geometry, transformLaserScanToPointCloud2)
00540 {
00541 
00542   tf::Transformer tf;
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, tf, -1.0, laser_geometry::channel_option::Index);
00617   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00618   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
00619   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00620 
00621   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00622   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00623   projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00624   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00625 
00626   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);
00627   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00628 
00629   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);
00630   EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00631 
00632   EXPECT_EQ(cloud_out.is_dense, false);
00633 
00634   unsigned int valid_points = 0;
00635   for (unsigned int i = 0; i < scan.ranges.size(); i++)
00636     if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00637       valid_points ++;    
00638   EXPECT_EQ(valid_points, cloud_out.width);
00639 
00640   uint32_t x_offset = 0;
00641   uint32_t y_offset = 0;
00642   uint32_t z_offset = 0;
00643   uint32_t intensity_offset = 0;
00644   uint32_t index_offset = 0;
00645   uint32_t distance_offset = 0;
00646   uint32_t stamps_offset = 0;
00647   for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00648   {
00649     if (f->name == "x") x_offset = f->offset;
00650     if (f->name == "y") y_offset = f->offset;
00651     if (f->name == "z") z_offset = f->offset;
00652     if (f->name == "intensity") intensity_offset = f->offset;
00653     if (f->name == "index") index_offset = f->offset;
00654     if (f->name == "distances") distance_offset = f->offset;
00655     if (f->name == "stamps") stamps_offset = f->offset;
00656   }
00657 
00658   for (unsigned int i = 0; i < cloud_out.width; i++)
00659   {
00660     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);
00661     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);
00662     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00663     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
00664     EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
00665     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
00666     EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
00667 
00668   };
00669     }
00670     catch (BuildScanException &ex)
00671     {
00672         if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
00673             FAIL();
00674     }
00675   }
00676 
00677 }
00678 
00679 
00680 int main(int argc, char **argv){
00681   ros::Time::init();
00682   testing::InitGoogleTest(&argc, argv);
00683   return RUN_ALL_TESTS();
00684 }
projection_test.cpp
