RGBDSLAM ICP fallback when there are not enough features

edit

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 ...