DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Unit tests are actually passing now

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35276 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs 2011-01-16 08:42:54 +00:00
parent ae9141d36d
commit 9c8817ba97
3 changed files with 69 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/laser_geometry/laser_geometry.h
View File

@ -291,7 +291,7 @@ namespace laser_geometry
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_; std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
float angle_min_; float angle_min_;
float angle_max_; float angle_max_;
// Eigen3::ArrayXXd co_sine_map_; Eigen3::ArrayXXd co_sine_map_;
boost::mutex guv_mutex_; boost::mutex guv_mutex_;
}; };

37
src/laser_geometry.cpp
View File

@ -291,13 +291,10 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
} }
// Check if our existing co_sine_map is valid // 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 ) 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."); 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_min_ = scan_in.angle_min;
angle_max_ = scan_in.angle_max; angle_max_ = scan_in.angle_max;
// Spherical->Cartesian projection // Spherical->Cartesian projection
@ -306,10 +303,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment); 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); co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
} }
#if 0
} }
#endif
output = ranges * co_sine_map_; output = ranges * co_sine_map_;
@ -319,14 +313,20 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.width = scan_in.ranges.size (); cloud_out.width = scan_in.ranges.size ();
cloud_out.fields.resize (3); cloud_out.fields.resize (3);
cloud_out.fields[0].name = "x"; 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].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].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 // 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; 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 //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) if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
{ {
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
@ -404,18 +404,17 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//TODO: Find out why this was needed //TODO: Find out why this was needed
//float bad_point = std::numeric_limits<float>::quiet_NaN (); //float bad_point = std::numeric_limits<float>::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; unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i) 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 //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]; float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
// Copy XYZ // Copy XYZ
@ -429,7 +428,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//Copy index //Copy index
if(idx_index != -1) if(idx_index != -1)
pstep[idx_index] = i; ((int*)(pstep))[idx_index] = i;
// Copy distance // Copy distance
if(idx_distance != -1) if(idx_distance != -1)
@ -637,14 +636,16 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; 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); 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 //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)) 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 //make sure to actually set the output

100
test/projection_test.cpp
View File

@ -353,7 +353,7 @@ TEST(laser_geometry, projectLaser2)
{ {
try 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::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
sensor_msgs::PointCloud2 cloud_out; sensor_msgs::PointCloud2 cloud_out;
@ -385,7 +385,7 @@ TEST(laser_geometry, projectLaser2)
uint32_t z_offset = 0; uint32_t z_offset = 0;
uint32_t intensity_offset = 0; uint32_t intensity_offset = 0;
uint32_t index_offset = 0; uint32_t index_offset = 0;
uint32_t ranges_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) for (std::vector<sensor_msgs::PointField>::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 == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_offset = f->offset; if (f->name == "intensity") intensity_offset = f->offset;
if (f->name == "index") index_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; if (f->name == "stamps") stamps_offset = f->offset;
} }
for (unsigned int i = 0; i < cloud_out.width; i++) 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(*(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(cloud_out.points[i].y , (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 + y_offset] , (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(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 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(*(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(cloud_out.channels[1].values[i], i, tolerance);//index EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], 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 + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
*/
}; };
} }
catch (BuildScanException &ex) catch (BuildScanException &ex)
@ -612,23 +611,23 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
sensor_msgs::PointCloud2 cloud_out; sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::None); 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)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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); 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); 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)3); 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);
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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); 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); 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); EXPECT_EQ(cloud_out.is_dense, false);
@ -638,36 +637,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
valid_points ++; valid_points ++;
EXPECT_EQ(valid_points, cloud_out.width); EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0; uint32_t x_offset = 0;
uint32_t y_offset = 0; uint32_t y_offset = 0;
uint32_t z_offset = 0; uint32_t z_offset = 0;
uint32_t intensity_offset = 0; uint32_t intensity_offset = 0;
uint32_t index_offset = 0; uint32_t index_offset = 0;
uint32_t ranges_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
{ {
if (f->name == "x") x_offset = f->offset; if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset; if (f->name == "y") y_offset = f->offset;
if (f->name == "z") z_offset = f->offset; if (f->name == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_offset = f->offset; if (f->name == "intensity") intensity_offset = f->offset;
if (f->name == "index") index_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; if (f->name == "stamps") stamps_offset = f->offset;
} }
for (unsigned int i = 0; i < cloud_out.width; i++) 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) catch (BuildScanException &ex)
@ -676,6 +673,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
FAIL(); FAIL();
} }
} }
} }