Ask Your Question
2

RGBDSLAM ICP fallback when there are not enough features

asked 2013-05-23 02:42:54 -0500

goetz.marc gravatar image

updated 2014-01-28 17:16:37 -0500

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
3

answered 2013-05-24 05:05:23 -0500

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.

edit flag offensive delete link more

Comments

compiling now works fine, thank you

goetz.marc gravatar imagegoetz.marc ( 2013-05-26 22:14:39 -0500 )edit

Let me know how ICP works out for you

Felix Endres gravatar imageFelix Endres ( 2013-05-26 23:13:09 -0500 )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

goetz.marc gravatar imagegoetz.marc ( 2013-05-28 22:29:13 -0500 )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!

Phelipe gravatar imagePhelipe ( 2015-01-16 07:49:53 -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: 2013-05-23 02:42:54 -0500

Seen: 417 times

Last updated: May 24 '13