Strange FOV error in laser_scan_matcher using a cloud
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;