0
$\begingroup$

Rosanswers logo

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);
    voxel.setInputCloud(cloud_2);
    voxel.filter(*cloud_2);
    // Set the input source and target
    icp->setInputCloud(cloud_1);
    icp->setInputTarget(cloud_2);
    // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    icp->setMaxCorrespondenceDistance(0.05);
    // Set the maximum number of iterations (criterion 1)
    icp->setMaximumIterations(50);
    // Set the transformation epsilon (criterion 2)
    icp->setTransformationEpsilon(1e-8);
    // Set the euclidean distance difference epsilon (criterion 3)
    icp->setEuclideanFitnessEpsilon(1);
    // Perform the alignment
    pointcloud_type::Ptr final;
    icp->align(*final);
    if (icp->hasConverged()) {
    publishCloud(new_node, now, batch_cloud_pub_);
    delete icp;
    process_node_runs_ = false;
    return true;
    }else{
    delete icp;
    process_node_runs_ = false;
    return false;
    }

But I do get a nice "process has died"-error when icp->align() is called.

Any suggestions/hints?

Thanks!


Originally posted by goetz.marc on ROS Answers with karma: 203 on 2013-05-23

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Felix Endres with karma: 6468 on 2013-05-24

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by goetz.marc on 2013-05-26:
compiling now works fine, thank you

Comment by Felix Endres on 2013-05-26:
Let me know how ICP works out for you

Comment by goetz.marc on 2013-05-28:
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

Comment by Phelipe on 2015-01-16:
@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!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.