Ask Your Question
1

Strange FOV error in laser_scan_matcher using a cloud

asked 2012-03-19 01:43:15 -0500

updated 2012-03-19 04:33:46 -0500

Hi, I am trying to use laser_scan_matcher with a PointCloud2 made by 2 opposite LaserScans, so I have a 360º pointcloud.

Is it possible?
When I run laser_scan_matcher i get the following error: (where FOV remains negative with different values)

Error in scan matching Strange FOV: -0.475675 rad = -27.254197 deg

I've tried to find the origin of the problem,

In the CSM laser_data.c code you can find it comes from this min and max theta:

196 double min_fov = deg2rad(20.0); 
197 double max_fov = 2.01 * M_PI;
198 double fov = ld->max_theta - ld->min_theta;
199 if( fov < min_fov || fov > max_fov) {
200   sm_error("Strange FOV: %f rad = %f deg \n", fov, rad2deg(fov));
201   return 0;
202 }

And from laser_scan_matcher.cpp:

532     ldp->theta[i] = atan2(cloud->points[i].y, cloud->points[i].x);
533     ldp->cluster[i]  = -1;
534   }
535 
536   ldp->min_theta = ldp->theta[0];
537   ldp->max_theta = ldp->theta[n-1];

So I suppose that the indexes 0 and n-1 of my cloud are not the right ones or when I create the cloud with the two LaserScans I am doing something wrong (even the cloud looks fine in rviz)

Anyone know a way to solve this?

thanks in advance!

Edit: How i construct the pointcloud

I have two subscribers to LaserScans, in their callbacks:

laser_projector_.projectLaser(*msg, cloud);
pcl_ros::transformPointCloud (base_frame_, cloud, cloudX_, tf_listener_);

When both messages have arrived and have been transformed I add them doing this:

std::vector<unsigned char> all_data(cloud1_.data);
all_data.reserve(cloud1_.data.size() + cloud2_.data.size());
all_data.insert(all_data.end(), cloud2_.data.begin(), cloud2_.data.end());

PointCloud2_msg_.header.seq      = counter_++;
PointCloud2_msg_.header.stamp    = ros::Time::now();
PointCloud2_msg_.header.frame_id = base_frame;
PointCloud2_msg_.fields          = cloud1_.fields;
PointCloud2_msg_.is_bigendian    = cloud1_.is_bigendian;
PointCloud2_msg_.point_step      = cloud1_.point_step;
PointCloud2_msg_.row_step        = cloud1_.row_step;
PointCloud2_msg_.is_dense        = cloud1_.is_dense;
PointCloud2_msg_.height          = cloud1_.height;
PointCloud2_msg_.width           = cloud1_.width+cloud2_.width;
PointCloud2_msg_.data            = all_data;
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2012-03-19 04:22:33 -0500

updated 2012-03-19 04:23:09 -0500

Make sure that your cloud is ordered sequentially - points[0] should correspond to the minimum-angle beam, and points[n-t] should be the maximum-angle beam.

You are getting a negative field of view (-27 deg), so that points to something wrong with the limits. How are you constructing the point cloud?

edit flag offensive delete link more

Comments

I have edited my question to show how i construct this pointcloud. thanks!

martimorta gravatar image martimorta  ( 2012-03-19 04:34:40 -0500 )edit

Make sure cloud 1 is on the right, and cloud2 on the left. Cloud1 should go from -pi to 0, and cloud 2 from 0 to pi. Can you verify the atan2 angles: cloud1.points[0] -> -pi, cloud2.points[<last>] -> pi?

Ivan Dryanovski gravatar image Ivan Dryanovski  ( 2012-03-19 04:49:24 -0500 )edit

It seems that cloud1 goes from ~-80 to ~80 through 0 and cloud2 from ~100 to ~-100 through 180 where it changes to -180. I suppose I should shift to get something like -pi..0 and 0..pi. thanks!

martimorta gravatar image martimorta  ( 2012-03-19 06:26:43 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-03-19 01:43:15 -0500

Seen: 706 times

Last updated: Mar 19 '12