# RGBDSLAM ICP fallback when there are not enough features

I'm currently using RGBDSLAM for mapping and autonomous driving. The room I'm trying to map has a big white wall, of course there are not enough features to be detected/extracted (<10), so RGBDSLAM tells me that it didn't add this node ("Found only %i features on image, node is not included").

So my question is: is the functionality for falling back to ICP only(!), if there are not enough features in the 2d image already implemented and if so, what am I doing wrong in the config/CMakeLists.txt ?

I first tried to activate ICP in the CMakieLists.txt, and in the launch file

• < param name="config/use_icp" value="true" />
• < param name="config/icp_method" value="icp" />
• set(USE_GICP_BIN 1)
• set(USE_GICP_CODE 0)
• set(USE_PCL_ICP 1)

Do i need to pick only one of these or do I have to combine them in some way?

Using only USE_PCL_ICP 1 doesn't seem to have an effect. Additionaly setting USE_GICP_BIN 1 is the same. Setting USE_GICP_CODE 1 leads to a compile error:

/home/sarcoma/groovy/sandbox/rgbdslam/rgbdslam/src/node.cpp:1221:114: error: ‘getRelativeTransformationTo_ICP_code’ was not declared in this scope


So then I took a look at the code.

The comment "///Apply Feature based Alignment and/or ICP" in node.cpp suggests, that (G)ICP was indeed intended to be used to improve the feature-based transformation estimation, or to replace it (when no features are found?)

In graph_manager.cpp I found the origion of the "node is not included" message:

if ((int)new_node->feature_locations_2d_.size() < ps->get<int>("min_matches") &&
! ps->get<bool>("keep_all_nodes"))


Setting the "keep_all_nodes" option in the launch file sure avoided the problem but, yeah, didn't help, because the new pointclouds were added all in the same position ("no-motion assumption"). Decreasing the "min_matches" option to <10 only drastically decreased the quality of the output.

Commenting out said if-statement didn't help either.

Another thing I found in node.cpp:

#if  defined USE_ICP_CODE || defined USE_ICP_CODE
///ICP - This sets the icp transformation in "mr", if the icp alignment is better than the ransac_quality
found_transformation = found_transformation || edge_from_icp_alignment(found_transformation, this, older_node, mr, ransac_quality);
#endif


So, when USE_ICP_CODE is defined (which it is, if USE_GICP_BIN or USE_GICP_CODE are set to 1?), the function edge_from_icp_alignment should be called, which should produce the transformation estimation even without features?

I also tried "manually" doing ICP when the if-statement for not enough features is triggered:

    if ((int)new_node->feature_locations_2d_.size() < ps->get<int>("min_matches") &&
! ps->get<bool>("keep_all_nodes"))
{
//magic?
ROS_INFO("Found only %i features on image, initializing ICP fallback",(int)new_node->feature_locations_2d_.size());
pcl::VoxelGrid<point_type> voxel;
new_node->id_ = graph_.size();
ros::Time now = ros::Time::now();
std::vector<int> indices;
pointcloud_type::Ptr cloud_1;
pointcloud_type::Ptr cloud_2;
pcl::IterativeClosestPoint<point_type, point_type>* icp = new pcl::IterativeClosestPoint<point_type, point_type>();
Node* old_node = graph_[graph_.size()-1];
cloud_1 = old_node->pc_col;
cloud_2 = new_node->pc_col;
pcl::removeNaNFromPointCloud(*cloud_1, *cloud_1, indices);
pcl::removeNaNFromPointCloud(*cloud_2, *cloud_2, indices);
voxel.setLeafSize(0.01, 0.01, 0.01);
voxel.setInputCloud(cloud_1);
voxel.filter(*cloud_1 ...
edit retag close merge delete

Sort by » oldest newest most voted

Hi. ICP support is constantly changing, with me thinking I can get it to improve results and ICP not delivering good results :-). Anyway, USE_ICP_BIN is outdated, we used to execute Segal's gicp as an external program.

I fixed the compile-time error, so try updating. You can leave USE_ICP_CODE and USE_PCL_ICP both set to 1 now.

However, the results are still almost always disappointing. I am using a rather old version of pcl though, they might have improved it.

more

compiling now works fine, thank you

( 2013-05-26 22:14:39 -0600 )edit

Let me know how ICP works out for you

( 2013-05-26 23:13:09 -0600 )edit

honestly i can't really see any difference, neither good nor bad; but i'm using rgbdslam in headless-mode (so no GUI) and only display the generated octomap (res: 0.075), which might be a reason that i'm not noticing any changes

( 2013-05-28 22:29:13 -0600 )edit

@goetz.marc Could you give more information about the mapping of big white walls using ICP algorithm? I'm facing the same problem (related just to numbers of features in the image and adding this as node, compilation is working) and that'll be great to know you did that! Thanks!

( 2015-01-16 07:49:53 -0600 )edit