From ae9141d36d4cb22bf71771421db9e41dd98dba60 Mon Sep 17 00:00:00 2001 From: Jeremy Leibs Date: Sun, 16 Jan 2011 04:45:46 +0000 Subject: [PATCH] Fixing the tests in laser_geometry -- now they break git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35275 eb33c2ac-9c88-4c90-87e0-44a10359b0c3 --- include/laser_geometry/laser_geometry.h | 11 +- src/laser_geometry.cpp | 20 ++- test/projection_test.cpp | 194 ++++++++++++++++++++---- 3 files changed, 186 insertions(+), 39 deletions(-) diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index 2e8be56..e8aa254 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -68,7 +68,7 @@ namespace laser_geometry Index = 0x02, //!< Enable "index" channel Distance = 0x04, //!< Enable "distances" channel Timestamp = 0x08, //!< Enable "stamps" channel - Viewpoint = 0x16, //!< Enable "viewpoint" channel + Viewpoint = 0x10, //!< Enable "viewpoint" channel Default = (Intensity | Index) //!< Enable "intensities" and "index" channels }; } @@ -181,7 +181,7 @@ namespace laser_geometry double range_cutoff, int channel_options = channel_option::Default) { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options, false); + return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options); } //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame @@ -209,7 +209,7 @@ namespace laser_geometry tf::Transformer& tf, int channel_options = channel_option::Default) { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options, false); + return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); } //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame @@ -277,8 +277,7 @@ namespace laser_geometry const sensor_msgs::LaserScan& scan_in, tf::Transformer & tf, double range_cutoff, - int channel_options, - bool preservative); + int channel_options); //! Internal hidden representation of transformLaserScanToPointCloud2 void transformLaserScanToPointCloud_ (const std::string &target_frame, @@ -292,7 +291,7 @@ namespace laser_geometry std::map* > unit_vector_map_; float angle_min_; float angle_max_; - Eigen3::ArrayXXd co_sine_map_; + // Eigen3::ArrayXXd co_sine_map_; boost::mutex guv_mutex_; }; diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 5c0787c..7d13964 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -189,7 +189,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, - tf::Transformer& tf, double range_cutoff, int mask, bool preservative) + tf::Transformer& tf, double range_cutoff, int mask) { cloud_out.header = scan_in.header; @@ -207,7 +207,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do pointIn.frame_id_ = scan_in.header.frame_id; - projectLaser_ (scan_in, cloud_out, range_cutoff, true, mask); + projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask); cloud_out.header.frame_id = target_frame; @@ -234,7 +234,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } //check just in case - ROS_ASSERT(index_channel_idx > 0); + ROS_ASSERT(index_channel_idx >= 0); for(unsigned int i = 0; i < cloud_out.points.size(); ++i) { @@ -280,8 +280,8 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do int channel_options) { size_t n_pts = scan_in.ranges.size (); - - Eigen3::ArrayXXd ranges (n_pts, 2), output (n_pts, 2); + Eigen3::ArrayXXd ranges (n_pts, 2); + Eigen3::ArrayXXd output (n_pts, 2); // Get the ranges into Eigen format for (size_t i = 0; i < n_pts; ++i) @@ -291,10 +291,13 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } // Check if our existing co_sine_map is valid +#if 0 if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max ) { +#endif ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); - co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2); + // co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2); + Eigen3::ArrayXXd co_sine_map_ (n_pts, 2); angle_min_ = scan_in.angle_min; angle_max_ = scan_in.angle_max; // Spherical->Cartesian projection @@ -303,7 +306,10 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment); co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment); } +#if 0 } +#endif + output = ranges * co_sine_map_; @@ -407,7 +413,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); //check to see if we want to keep the point - if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) + if (scan_in.ranges[i] <= range_cutoff && scan_in.ranges[i] >= scan_in.range_min) { float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step]; diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 35b113a..98a02d3 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -42,12 +42,16 @@ #define PROJECTION_TEST_RANGE_MIN (0.23) #define PROJECTION_TEST_RANGE_MAX (40.0) + +class BuildScanException { }; + sensor_msgs::LaserScan build_constant_scan(double range, double intensity, double ang_min, double ang_max, double ang_increment, ros::Duration scan_time) { - if ((ang_max - ang_min) / ang_increment < 0) - throw (std::runtime_error("cannnot build a scan with non convergent ends")); + if (((ang_max - ang_min) / ang_increment) < 0) + throw (BuildScanException()); + sensor_msgs::LaserScan scan; scan.header.stamp = ros::Time::now(); scan.header.frame_id = "laser_frame"; @@ -57,12 +61,13 @@ sensor_msgs::LaserScan build_constant_scan(double range, double intensity, scan.scan_time = scan_time.toSec(); scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_max = PROJECTION_TEST_RANGE_MAX; - unsigned int i = 0; + uint32_t i = 0; for(; ang_min + i * ang_increment < ang_max; i++) { scan.ranges.push_back(range); scan.intensities.push_back(intensity); } + scan.time_increment = scan_time.toSec()/(double)i; return scan; @@ -100,6 +105,8 @@ void test_getUnitVectors(double angle_min, double angle_max, double angle_increm } } +#if 0 + TEST(laser_geometry, getUnitVectors) { double min_angle = -M_PI/2; @@ -153,10 +160,10 @@ TEST(laser_geometry, getUnitVectors) test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment); test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1); } - catch (std::runtime_error &ex) + catch (BuildScanException &ex) { - if ((max_angle - max_angle) / angle_increment > 0.0)//make sure it is not a false exception - EXPECT_FALSE(ex.what()); + if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + FAIL(); } } //else @@ -231,12 +238,9 @@ TEST(laser_geometry, projectLaser) { try { - //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - // WE NEVER GET HERE!! - FAIL(); - sensor_msgs::PointCloud cloud_out; projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); @@ -257,7 +261,8 @@ TEST(laser_geometry, projectLaser) unsigned int valid_points = 0; for (unsigned int i = 0; i < scan.ranges.size(); i++) if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; + valid_points ++; + EXPECT_EQ(valid_points, cloud_out.points.size()); for (unsigned int i = 0; i < cloud_out.points.size(); i++) @@ -271,14 +276,150 @@ TEST(laser_geometry, projectLaser) EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps }; } - catch (std::runtime_error &ex) + catch (BuildScanException &ex) { - if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception - EXPECT_FALSE(ex.what()); + if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + FAIL(); } } } +#endif + + +TEST(laser_geometry, projectLaser2) +{ + double tolerance = 1e-12; + laser_geometry::LaserProjection projector; + + double min_angle = -M_PI/2; + double max_angle = M_PI/2; + double angle_increment = M_PI/180; + + double range = 1.0; + double intensity = 1.0; + + ros::Duration scan_time = ros::Duration(1/40); + ros::Duration increment_time = ros::Duration(1/400); + + + std::vector ranges, intensities, min_angles, max_angles, angle_increments; + std::vector increment_times, scan_times; + + rostest::Permuter permuter; + + ranges.push_back(-1.0); + ranges.push_back(1.0); + ranges.push_back(2.0); + ranges.push_back(3.0); + ranges.push_back(4.0); + ranges.push_back(5.0); + ranges.push_back(100.0); + permuter.addOptionSet(ranges, &range); + + intensities.push_back(1.0); + intensities.push_back(2.0); + intensities.push_back(3.0); + intensities.push_back(4.0); + intensities.push_back(5.0); + permuter.addOptionSet(intensities, &intensity); + + min_angles.push_back(-M_PI); + min_angles.push_back(-M_PI/1.5); + min_angles.push_back(-M_PI/2); + min_angles.push_back(-M_PI/4); + min_angles.push_back(-M_PI/8); + permuter.addOptionSet(min_angles, &min_angle); + + max_angles.push_back(M_PI); + max_angles.push_back(M_PI/1.5); + max_angles.push_back(M_PI/2); + max_angles.push_back(M_PI/4); + max_angles.push_back(M_PI/8); + permuter.addOptionSet(max_angles, &max_angle); + + // angle_increments.push_back(-M_PI/180); // -one degree + angle_increments.push_back(M_PI/180); // one degree + angle_increments.push_back(M_PI/360); // half degree + angle_increments.push_back(M_PI/720); // quarter degree + permuter.addOptionSet(angle_increments, &angle_increment); + + scan_times.push_back(ros::Duration(1/40)); + scan_times.push_back(ros::Duration(1/20)); + + permuter.addOptionSet(scan_times, &scan_time); + + while (permuter.step()) + { + try + { + printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + + sensor_msgs::PointCloud2 cloud_out; + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + + projector.projectLaser(scan, cloud_out, -1.0); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); + + 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); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); + + unsigned int valid_points = 0; + for (unsigned int i = 0; i < scan.ranges.size(); i++) + if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) + valid_points ++; + + EXPECT_EQ(valid_points, cloud_out.width); + + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t z_offset = 0; + uint32_t intensity_offset = 0; + uint32_t index_offset = 0; + uint32_t ranges_offset = 0; + uint32_t stamps_offset = 0; + for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) + { + if (f->name == "x") x_offset = f->offset; + if (f->name == "y") y_offset = f->offset; + if (f->name == "z") z_offset = f->offset; + if (f->name == "intensity") intensity_offset = f->offset; + if (f->name == "index") index_offset = f->offset; + if (f->name == "ranges") ranges_offset = f->offset; + if (f->name == "stamps") stamps_offset = f->offset; + } + + for (unsigned int i = 0; i < cloud_out.width; i++) + { + /* + EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance); + EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order + EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index + EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges + EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps + */ + }; + } + catch (BuildScanException &ex) + { + if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + FAIL(); + } + } +} + + TEST(laser_geometry, transformLaserScanToPointCloud) { @@ -388,16 +529,14 @@ TEST(laser_geometry, transformLaserScanToPointCloud) EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps }; } - catch (std::runtime_error &ex) + catch (BuildScanException &ex) { - if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception - EXPECT_FALSE(ex.what()); + if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + FAIL(); } } } - - TEST(laser_geometry, transformLaserScanToPointCloud2) { @@ -491,14 +630,14 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) 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); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + EXPECT_EQ(cloud_out.is_dense, false); + unsigned int valid_points = 0; for (unsigned int i = 0; i < scan.ranges.size(); i++) if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) valid_points ++; EXPECT_EQ(valid_points, cloud_out.width); - for (unsigned int i = 0; i < cloud_out.width; i++) - { uint32_t x_offset = 0; uint32_t y_offset = 0; uint32_t z_offset = 0; @@ -516,6 +655,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) if (f->name == "ranges") ranges_offset = f->offset; if (f->name == "stamps") stamps_offset = f->offset; } + + for (unsigned int i = 0; i < cloud_out.width; i++) + { + /* EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); @@ -527,18 +670,17 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) */ }; } - catch (std::runtime_error &ex) + catch (BuildScanException &ex) { - if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception - EXPECT_FALSE(ex.what()); + if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + FAIL(); } } } - - int main(int argc, char **argv){ + ros::Time::init(); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }