Visualizer displaying model pointclouds incorrectly in correspondance grouping tutorial

[mwcondino]

I was experimenting with the stock correspondance_grouping.cpp code and noticed an issue with the visualizer. I modified the code by changing the pointtype to XYZ and removing the uniform filtering so that the *model_keypoints and *scene_keypoints clouds were identical to *model and *scene respectively. I was running tests to identify a backpack in a sparse pointcloud that I created, and was using a model of the backpack that I made by cropping out the rest of the scene. I was able to get the code to run by using the geometric consistency grouping method, and the software was able to succesfully identify 814 correspondances.

However, when I'm visualizing the scene and model clouds using the stock visualization code that came with the tutorial, I'm seeing the following display:

It's clear that the identified model cloud, and even the off-model cloud (which should only be shifted by 1m in the x direction), are being displayed incorrectly, even though the green lines which display the correspondences between the model cloud and scene cloud are in the correct position.

I think that there's an issue with how the pointclouds are being added to the visualizer. To verify this, I took the pointclouds for the scene, the rotated model, and the off-scene model and published this data as ROS pointcloud2 messages. When I visualize this data in RVIZ, the model (shown in purple) and off-scene model (shown in yellow) are in the correct positions:

Your Environment

I am currently running PCL 1.7.2-14build1 on an amd64 machine, which I originally installed via apt (apt-get install libpcl-dev). My ROS version is Lunar, and for the record, I built and compiled the correspondance_grouping code within a ROS package using catkin. I am running Ubuntu 16.04.

Context

Expected Behavior

Current Behavior

Code to Reproduce

Here's my raw code:
`
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
//#include <pcl/filters/uniform_sampling.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

//typedef pcl::PointXYZRGBA PointType;
typedef pcl::PointXYZ PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;

std::string model_filename_;
std::string scene_filename_;

//Algorithm params
bool show_keypoints_ (false);
bool show_correspondences_ (false);
bool use_cloud_resolution_ (false);
bool use_hough_ (true);
float model_ss_ (0.01f);
float scene_ss_ (0.03f);
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);
float cg_thresh_ (5.0f);

void
showHelp (char filename)
{
std::cout << std::endl;
std::cout << "" << std::endl;
std::cout << " " << std::endl;
std::cout << " Correspondence Grouping Tutorial - Usage Guide " << std::endl;
std::cout << " " << std::endl;
std::cout << "*" << std::endl << std::endl;
std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
std::cout << "Options:" << std::endl;
std::cout << " -h: Show this help." << std::endl;
std::cout << " -k: Show used keypoints." << std::endl;
std::cout << " -c: Show used correspondences." << std::endl;
std::cout << " -r: Compute the model cloud resolution and multiply" << std::endl;
std::cout << " each radius given by that value." << std::endl;
std::cout << " --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
std::cout << " --model_ss val: Model uniform sampling radius (default 0.01)" << std::endl;
std::cout << " --scene_ss val: Scene uniform sampling radius (default 0.03)" << std::endl;
std::cout << " --rf_rad val: Reference frame radius (default 0.015)" << std::endl;
std::cout << " --descr_rad val: Descriptor radius (default 0.02)" << std::endl;
std::cout << " --cg_size val: Cluster size (default 0.01)" << std::endl;
std::cout << " --cg_thresh val: Clustering threshold (default 5)" << std::endl << std::endl;
}

void
parseCommandLine (int argc, char *argv[])
{
//Show help
if (pcl::console::find_switch (argc, argv, "-h"))
{
showHelp (argv[0]);
exit (0);
}

//Model & scene filenames
std::vector filenames;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 2)
{
std::cout << "Filenames missing.\n";
showHelp (argv[0]);
exit (-1);
}

model_filename_ = argv[filenames[0]];
scene_filename_ = argv[filenames[1]];

//Program behavior
if (pcl::console::find_switch (argc, argv, "-k"))
{
show_keypoints_ = true;
}
if (pcl::console::find_switch (argc, argv, "-c"))
{
show_correspondences_ = true;
}
if (pcl::console::find_switch (argc, argv, "-r"))
{
use_cloud_resolution_ = true;
}

std::string used_algorithm;
if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
{
if (used_algorithm.compare ("Hough") == 0)
{
use_hough_ = true;
}else if (used_algorithm.compare ("GC") == 0)
{
use_hough_ = false;
}
else
{
std::cout << "Wrong algorithm name.\n";
showHelp (argv[0]);
exit (-1);
}
}

//General parameters
pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
}

double
computeCloudResolution (const pcl::PointCloud::ConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector indices (2);
std::vector sqr_distances (2);
pcl::search::KdTree tree;
tree.setInputCloud (cloud);

for (size_t i = 0; i < cloud->size (); ++i)
{
if (! pcl_isfinite ((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}

int
main (int argc, char *argv[])
{
parseCommandLine (argc, argv);

pcl::PointCloud::Ptr model (new pcl::PointCloud ());
pcl::PointCloud::Ptr model_keypoints (new pcl::PointCloud ());
pcl::PointCloud::Ptr scene (new pcl::PointCloud ());
pcl::PointCloud::Ptr scene_keypoints (new pcl::PointCloud ());
pcl::PointCloud::Ptr model_normals (new pcl::PointCloud ());
pcl::PointCloud::Ptr scene_normals (new pcl::PointCloud ());
pcl::PointCloud::Ptr model_descriptors (new pcl::PointCloud ());
pcl::PointCloud::Ptr scene_descriptors (new pcl::PointCloud ());

//
// Load clouds
//
if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}

//
// Set up resolution invariance
//
if (use_cloud_resolution_)
{
float resolution = static_cast (computeCloudResolution (model));
if (resolution != 0.0f)
{
model_ss_ *= resolution;
scene_ss_ *= resolution;
rf_rad_ *= resolution;
descr_rad_ *= resolution;
cg_size_ *= resolution;
}

std::cout << "Model resolution:       " << resolution << std::endl;
std::cout << "Model sampling size:    " << model_ss_ << std::endl;
std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;

}

//
// Compute Normals
//
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);

norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);

//
// Downsample Clouds to Extract keypoints
//

/*
pcl::UniformSampling uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
//uniform_sampling.filter (*model_keypoints);
pcl::PointCloud keypointIndices1;
uniform_sampling.compute(keypointIndices1);
pcl::copyPointCloud(*model, keypointIndices1.points, *model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
//uniform_sampling.filter (*scene_keypoints);
pcl::PointCloud keypointIndices2;
uniform_sampling.compute(keypointIndices2);
pcl::copyPointCloud(*scene, keypointIndices2.points, *scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
*/

*model_keypoints = *model;
*scene_keypoints = *scene;
//
// Compute Descriptor for keypoints
//
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
descr_est.setRadiusSearch (descr_rad_);

descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);

descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);

//
// Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());

pcl::KdTreeFLANN match_search;
match_search.setInputCloud (model_descriptors);

// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector neigh_indices (1);
std::vector neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[0], static_cast (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

//
// Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocatorEigen::Matrix4f > rototranslations;
std::vectorpcl::Correspondences clustered_corrs;

// Using Hough3D
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud::Ptr model_rf (new pcl::PointCloud ());
pcl::PointCloud::Ptr scene_rf (new pcl::PointCloud ());

pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);

rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);

rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);

//  Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);

clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);

//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);

}
else // Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);

gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);

}

//
// Output results
//
std::cout << "Model instances found: " << rototranslations.size () << std::endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;

// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);

printf ("\n");
printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));

}

//
// Visualization
//
pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
viewer.addPointCloud (scene, "scene_cloud");

usleep(500000);
viewer.addPointCloud (scene, "scene_cloud");

pcl::PointCloud::Ptr off_scene_model (new pcl::PointCloud ());
pcl::PointCloud::Ptr off_scene_model_keypoints (new pcl::PointCloud ());

if (show_correspondences_ || show_keypoints_)
{
// We are translating the model so that it doesn't end in the middle of the scene representation
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));

pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");

}

if (show_keypoints_)
{
pcl::visualization::PointCloudColorHandlerCustom scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");

pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");

}
pcl::PointCloud::Ptr rotated_model_permanent (new pcl::PointCloud ());
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud::Ptr rotated_model (new pcl::PointCloud ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
pcl::transformPointCloud (*model, *rotated_model_permanent, rototranslations[0]);
std::stringstream ss_cloud;
ss_cloud << "instance" << i;

pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());

if (show_correspondences_)
{
  for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
  {
    std::stringstream ss_line;
    ss_line << "correspondence_line" << i << "_" << j;
    PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
    PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);

    //  We are drawing a line for each pair of clustered correspondences found between the model and the scene
    viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
  }
}

}

//initialize ros... lol

ros::init(argc, argv, "correspondance_grouping");
ros::start();

ros::NodeHandle nh("~");

//create ros messages.
sensor_msgs::PointCloud2 sceneCloud, modelCloud, offSceneModelCloud;

//change point cloud, make it into a point cloud 2 message.
pcl::toROSMsg(*scene, sceneCloud);
pcl::toROSMsg(*rotated_model_permanent, modelCloud);
pcl::toROSMsg(*off_scene_model, offSceneModelCloud);
//set frame id of published message
sceneCloud.header.frame_id = "map";
modelCloud.header.frame_id = "map";
offSceneModelCloud.header.frame_id = "map";

//set up pubs.
ros::Publisher scenePub = nh.advertise<sensor_msgs::PointCloud2>("scene", 1);
ros::Publisher modelPub = nh.advertise<sensor_msgs::PointCloud2>("model", 1);
ros::Publisher offSceneModelPub = nh.advertise<sensor_msgs::PointCloud2>("offSceneModel", 1);

while (!viewer.wasStopped ())
{
viewer.spinOnce ();
//update time
sceneCloud.header.stamp = ros::Time::now();
modelCloud.header.stamp = ros::Time::now();
offSceneModelCloud.header.stamp = ros::Time::now();

scenePub.publish(sceneCloud);
modelPub.publish(modelCloud);
offSceneModelPub.publish(offSceneModelCloud);
ros::spinOnce();

}

ros::shutdown();

return (0);
}
`

Here's my project's CMakeLists file:
`cmake_minimum_required(VERSION 2.8.3)
project(monocular_slam_nav)

IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Debug)
ENDIF()

MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")

Check C++11 or C++0x support

include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

#catkin dependencies
find_package(catkin REQUIRED COMPONENTS
roscpp
pcl_ros
sensor_msgs
)

#set(Boost_USE_STATIC_LIBS ON)
add_definitions(-DBOOST_TEST_DYN_LINK)

System dependencies are found with CMake's conventions

find_package(Boost REQUIRED COMPONENTS
system
)

find_package(metslib REQUIRED)

catkin_package(
INCLUDE_DIRS include
LIBRARIES
CATKIN_DEPENDS roscpp pcl_ros sensor_msgs
DEPENDS metslib
)

include_directories(
${PROJECT_SOURCE_DIR}/include
${catkin_INCLUDE_DIRS}
${metslib_INCLUDE_DIRS}
)

set(LIBS
${catkin_LIBRARIES}
)

######################################

correspondance grouping example

add_executable(correspondance_grouping
src/correspondance_grouping.cpp
)

target_link_libraries(correspondance_grouping
${LIBS}
)

global hypothesis verification example

add_executable(global_hypothesis_verification
src/global_hypothesis_verification.cpp
)

target_link_libraries(global_hypothesis_verification
${LIBS}
)

add_executable(correspondance_grouping_original
src/correspondance_grouping_original.cpp
)

target_link_libraries(correspondance_grouping_original
${LIBS}
)
`

Here's my package.xml in case you want to recreate within a catkinized workspace:
`

monocular_slam_nav
0.0.0
The monocular_slam_nav package

rsl

TODO

<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>metslib</build_depend>
<build_depend>sensor_msgs</build_depend>

<exec_depend>roscpp</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>metslib</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

` ## Possible Solution

[stale]

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.