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 modelfilename; std::string scenefilename;

//Algorithm params bool showkeypoints (false); bool showcorrespondences (false); bool use_cloudresolution (false); bool usehough (true); float modelss (0.01f); float sceness (0.03f); float rfrad (0.015f); float descrrad (0.02f); float cgsize (0.01f); float cgthresh (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); }

modelfilename = argv[filenames[0]]; scenefilename = argv[filenames[1]];

//Program behavior if (pcl::console::find_switch (argc, argv, "-k")) { showkeypoints = true; } if (pcl::console::find_switch (argc, argv, "-c")) { showcorrespondences = true; } if (pcl::console::find_switch (argc, argv, "-r")) { use_cloudresolution = true; }

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

//General parameters pcl::console::parse_argument (argc, argv, "--model_ss", modelss); pcl::console::parse_argument (argc, argv, "--scene_ss", sceness); pcl::console::parse_argument (argc, argv, "--rf_rad", rfrad); pcl::console::parse_argument (argc, argv, "--descr_rad", descrrad); pcl::console::parse_argument (argc, argv, "--cg_size", cgsize); pcl::console::parse_argument (argc, argv, "--cg_thresh", cgthresh); }

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 (modelfilename, model) < 0) { std::cout << "Error loading model cloud." << std::endl; showHelp (argv[0]); return (-1); } if (pcl::io::loadPCDFile (scenefilename, scene) < 0) { std::cout << "Error loading scene cloud." << std::endl; showHelp (argv[0]); return (-1); }

// // Set up resolution invariance // if (use_cloudresolution) { float resolution = static_cast (computeCloudResolution (model)); if (resolution != 0.0f) { modelss = resolution; sceness = resolution; rfrad = resolution; descrrad = resolution; cgsize *= 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 (modelss); //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 (sceness); //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 (descrrad);

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_allocator > rototranslations; std::vector clustered_corrs;

// Using Hough3D if (usehough) { // // 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 (cgsize); gc_clusterer.setGCThreshold (cgthresh);

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 (showcorrespondences || showkeypoints) { // 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 (showkeypoints) { 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("scene", 1); ros::Publisher modelPub = nh.advertise("model", 1); ros::Publisher offSceneModelPub = nh.advertise("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: `<?xml version="1.0"?>

monocular_slam_nav 0.0.0 The monocular_slam_nav package rsl TODO catkin roscpp pcl_ros metslib sensor_msgs roscpp pcl_ros metslib sensor_msgs

`

Possible Solution

[stale[bot]]

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