From 9c8817ba974a03fee7860cd9e4926bba51e6cd7f Mon Sep 17 00:00:00 2001 From: Jeremy Leibs Date: Sun, 16 Jan 2011 08:42:54 +0000 Subject: [PATCH] Unit tests are actually passing now git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35276 eb33c2ac-9c88-4c90-87e0-44a10359b0c3 --- include/laser_geometry/laser_geometry.h | 2 +- src/laser_geometry.cpp | 37 ++++----- test/projection_test.cpp | 100 ++++++++++++------------ 3 files changed, 69 insertions(+), 70 deletions(-) diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index e8aa254..4c18b2f 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -291,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 7d13964..34230e9 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -291,13 +291,10 @@ 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); - Eigen3::ArrayXXd co_sine_map_ (n_pts, 2); + co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2); angle_min_ = scan_in.angle_min; angle_max_ = scan_in.angle_max; // Spherical->Cartesian projection @@ -306,10 +303,7 @@ 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_; @@ -319,14 +313,20 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.width = scan_in.ranges.size (); cloud_out.fields.resize (3); cloud_out.fields[0].name = "x"; + cloud_out.fields[0].offset = 0; + cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[1].name = "y"; + cloud_out.fields[1].offset = 4; + cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[2].name = "z"; + cloud_out.fields[2].offset = 8; + cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32; // Define 4 indices in the channel array for each possible value type int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1; //now, we need to check what fields we need to store - int offset = 0; + int offset = 12; if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) { int field_size = cloud_out.fields.size(); @@ -404,18 +404,17 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do //TODO: Find out why this was needed //float bad_point = std::numeric_limits::quiet_NaN (); + if (range_cutoff < 0) + range_cutoff = scan_in.range_max; + else + range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); + unsigned int count = 0; for (size_t i = 0; i < n_pts; ++i) { - if (range_cutoff < 0) - range_cutoff = scan_in.range_max; - else - 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) { - float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step]; // Copy XYZ @@ -429,7 +428,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do //Copy index if(idx_index != -1) - pstep[idx_index] = i; + ((int*)(pstep))[idx_index] = i; // Copy distance if(idx_distance != -1) @@ -637,14 +636,16 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height); + uint32_t i = 0; + uint32_t j = 0; //copy over the data from one cloud to the other - unsigned int i = 0, j = 0; - while(i < cloud_out.data.size()) + while (i < cloud_out.data.size()) { if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4)) { - cloud_without_index.data[j++] = cloud_out.data[i++]; + cloud_without_index.data[j++] = cloud_out.data[i]; } + i++; } //make sure to actually set the output diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 98a02d3..57cf857 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -353,7 +353,7 @@ TEST(laser_geometry, projectLaser2) { 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); sensor_msgs::PointCloud2 cloud_out; @@ -385,7 +385,7 @@ TEST(laser_geometry, projectLaser2) uint32_t z_offset = 0; uint32_t intensity_offset = 0; uint32_t index_offset = 0; - uint32_t ranges_offset = 0; + uint32_t distance_offset = 0; uint32_t stamps_offset = 0; for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { @@ -394,21 +394,20 @@ TEST(laser_geometry, projectLaser2) 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 == "distances") distance_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 - */ + + 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); + 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); + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); + 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 + EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps }; } catch (BuildScanException &ex) @@ -612,23 +611,23 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) scan.header.frame_id = "laser_frame"; sensor_msgs::PointCloud2 cloud_out; - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::None); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)1); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)2); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)2); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); - - 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); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - 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); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + + 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); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); + + 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); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); EXPECT_EQ(cloud_out.is_dense, false); @@ -638,36 +637,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) 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; - } + 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 distance_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 == "distances") distance_offset = f->offset; + if (f->name == "stamps") stamps_offset = f->offset; + } for (unsigned int i = 0; i < cloud_out.width; i++) { + 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); + 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); + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); + 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 + EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges + EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps - /* - 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.fields[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(cloud_out.fields[1].values[i], i, tolerance);//index - EXPECT_NEAR(cloud_out.fields[2].values[i], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(cloud_out.fields[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps - */ }; } catch (BuildScanException &ex) @@ -676,6 +673,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) FAIL(); } } + }