From 671ec17afee0e1b150d9a43a379b9fe8e07a356e Mon Sep 17 00:00:00 2001 From: Jeremy Leibs Date: Thu, 7 Jan 2010 19:29:45 +0000 Subject: [PATCH] Resolved some float->double conversion errors to fix the tolerances for #1651 git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26612 eb33c2ac-9c88-4c90-87e0-44a10359b0c3 --- include/laser_geometry/laser_geometry.h | 6 +++--- src/laser_geometry.cpp | 2 +- test/projection_test.cpp | 22 +++++++++++----------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index ef53bae..f58d4b1 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -209,9 +209,9 @@ namespace laser_geometry * it is left protected so that test code can evaluate it * appropriately. */ - const boost::numeric::ublas::matrix& getUnitVectors_(float angle_min, - float angle_max, - float angle_increment, + const boost::numeric::ublas::matrix& getUnitVectors_(double angle_min, + double angle_max, + double angle_increment, unsigned int length); private: diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index c4826e4..02aca3b 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -150,7 +150,7 @@ namespace laser_geometry cloud_out.channels[d].set_values_size(count); }; -const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(float angle_min, float angle_max, float angle_increment, unsigned int length) +const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length) { //construct string for lookup in the map std::stringstream anglestring; diff --git a/test/projection_test.cpp b/test/projection_test.cpp index e5be16d..e7b7230 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -72,18 +72,18 @@ sensor_msgs::LaserScan build_constant_scan(double range, double intensity, class TestProjection : public laser_geometry::LaserProjection { public: - const boost::numeric::ublas::matrix& getUnitVectors(float angle_min, - float angle_max, - float angle_increment, + const boost::numeric::ublas::matrix& getUnitVectors(double angle_min, + double angle_max, + double angle_increment, unsigned int length) { return getUnitVectors_(angle_min, angle_max, angle_increment, length); } }; -void test_getUnitVectors(float angle_min, float angle_max, float angle_increment, unsigned int length) +void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length) { - double tolerance = 1e-6; + double tolerance = 1e-12; TestProjection projector; const boost::numeric::ublas::matrix & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length); @@ -167,7 +167,7 @@ TEST(laser_geometry, getUnitVectors) TEST(laser_geometry, projectLaser) { - double tolerance = 1e-5; + double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; @@ -259,8 +259,8 @@ TEST(laser_geometry, projectLaser) for (unsigned int i = 0; i < cloud_out.points.size(); i++) { - EXPECT_NEAR(cloud_out.points[i].x , scan.ranges[i] * cos(scan.angle_min + i * scan.angle_increment), tolerance); - EXPECT_NEAR(cloud_out.points[i].y , scan.ranges[i] * sin(scan.angle_min + i * scan.angle_increment), tolerance); + 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 @@ -280,7 +280,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud) tf::Transformer tf; - double tolerance = 1e-5; + double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; @@ -375,8 +375,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud) for (unsigned int i = 0; i < cloud_out.points.size(); i++) { - EXPECT_NEAR(cloud_out.points[i].x , scan.ranges[i] * cos(scan.angle_min + i * scan.angle_increment), tolerance); - EXPECT_NEAR(cloud_out.points[i].y , scan.ranges[i] * sin(scan.angle_min + i * scan.angle_increment), tolerance); + 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