PCL good correspondence lines not touching the points between source and target point clouds

[preethamam]

Whenever I try to display the source and target point clouds (which have displaced with some R,T matrix initially) on the same PCL viewer, the correspondence lines of the good matching target keypoints are not touching the source keypoints.

Your Environment

• Operating System and version: Windows 10 1903
• Compiler: CMake 3.15, Visual Studio 17
• PCL Version: 1.9.1

Context

SIFT keypoints are used to perform the initial alignment of the two-point clouds (source--src and target--tgt), good matching target keypoints are not touching the source keypoints.

Please note that I have not explicitly transformed the source point cloud before displaying on the same viewer. Eventually, the PCL visualization library is transformation the target point cloud.

Source SIFT key points - Red color Target SIFT key points - Green color

Expected Behavior

Same visualization results using MATLAB: All the PCD files and keypoints are plotted using the data obtained from the PCL library.

Current Behavior

Code to Reproduce

#include <fstream>
#include <iostream>
#include <cstring>
#include <string>
#include <ctime>
#include <vector>
#include <sstream>

#include <pcl/common/angles.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/io.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/pfh.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/point_cloud_geometry_handlers.h>


#include <pcl/correspondence.h>
#include <pcl/console/time.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/pfh.h>
#include <pcl/features/pfhrgb.h>
#include <pcl/features/shot_omp.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/registration/correspondence_estimation_backprojection.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
#include <pcl/registration/default_convergence_criteria.h>


#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/transformation_estimation_svd.h>


using namespace std;

// Hyper parameters
#define LEAF_SIZE .1
#define normal_radius 0.25
#define feature_radius 0.25
#define RANSAC_Inlier_Threshold 0.1
#define RANSAC_Iterations 5000
#define CorrRejDist_Maximum_Distance 5
#define ICP_Max_Correspondence_Distance 0.15

// Parameters for sift computation
#define min_scale 0.2
#define nr_octaves 4
#define nr_scales_per_octave 5
#define min_contrast 0.25

void detect_keypoints(pcl::PointCloud <pcl::PointXYZRGB>::Ptr &points,
    pcl::PointCloud <pcl::PointWithScale>::Ptr &keypoints_out) {

    pcl::SIFTKeypoint <pcl::PointXYZRGB, pcl::PointWithScale> sift_detect;

    // Use a FLANN-based KdTree to perform neighbourhood searches
    pcl::search::KdTree <pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree <pcl::PointXYZRGB>);
    sift_detect.setSearchMethod(tree);

    // Set the detection parameters
    sift_detect.setScales(min_scale, nr_octaves, nr_scales_per_octave);
    sift_detect.setMinimumContrast(min_contrast);

    // Set the input
    sift_detect.setInputCloud(points);

    // Detect the keypoints and store them in "keypoints.out"
    sift_detect.compute(*keypoints_out);
}

void compute_normals(pcl::PointCloud <pcl::PointXYZRGB>::Ptr &points,
     pcl::PointCloud <pcl::Normal>::Ptr &normals_out) {

    pcl::NormalEstimation <pcl::PointXYZRGB, pcl::Normal> norm_est;
    // Use a FLANN-based KdTree to perform neighbourhood searches
    norm_est.setSearchMethod(pcl::search::KdTree <pcl::PointXYZRGB>::Ptr(new pcl::search::KdTree <pcl::PointXYZRGB>));

    norm_est.setRadiusSearch(normal_radius);
    norm_est.setInputCloud(points);
    norm_est.compute(*normals_out);
}

void compute_PFHRGB_features(pcl::PointCloud <pcl::PointXYZRGB>::Ptr &cloud,
    pcl::PointCloud <pcl::Normal>::Ptr &normals,
    pcl::PointCloud <pcl::PointWithScale>::Ptr &keypoints,
    pcl::PointCloud <pcl::PFHRGBSignature250>::Ptr &descriptors_out) {


    // copy only XYZ data of keypoints for use in estimating features
    pcl::PointCloud <pcl::PointXYZRGB>::Ptr keypoints_xyzrgb(new pcl::PointCloud <pcl::PointXYZRGB>);
    pcl::copyPointCloud(*keypoints, *keypoints_xyzrgb);

    // Create the PFH estimation class, and pass the input dataset+normals to it
    pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250> pfhrgbEstimation;


    pfhrgbEstimation.setInputCloud(keypoints_xyzrgb);
    pfhrgbEstimation.setSearchSurface(cloud); // use all points for analyzing local cloud structure 
    pfhrgbEstimation.setInputNormals(normals);
    // alternatively, if cloud is of tpe PointNormal, do pfh.setInputNormals (cloud);

    // Create an empty kdtree representation, and pass it to the PFH estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
    pfhrgbEstimation.setSearchMethod(tree);
    //pfhrgbEstimation.setKSearch(100);

    // Use all neighbors in a sphere of radius radius
    // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
    pfhrgbEstimation.setRadiusSearch(feature_radius);

    // Compute the features
    pfhrgbEstimation.compute(*descriptors_out);

}

void findCorrespondences_PFHRGB(const pcl::PointCloud<pcl::PFHRGBSignature250>::Ptr &fpfhs_src,
    const pcl::PointCloud<pcl::PFHRGBSignature250>::Ptr &fpfhs_tgt,
    pcl::Correspondences &all_correspondences) {

    pcl::registration::CorrespondenceEstimation<pcl::PFHRGBSignature250, pcl::PFHRGBSignature250> est;
    est.setInputSource(fpfhs_src);
    est.setInputTarget(fpfhs_tgt);
    est.determineReciprocalCorrespondences(all_correspondences);
}

void rejectBadCorrespondences(const pcl::CorrespondencesPtr &all_correspondences,
                              const pcl::PointCloud<pcl::PointWithScale>::Ptr &keypoints_src,
                              const pcl::PointCloud<pcl::PointWithScale>::Ptr &keypoints_tgt,
                                    pcl::Correspondences &remaining_correspondences)
{
    // copy only XYZRGB data of keypoints for use in estimating features
    pcl::PointCloud <pcl::PointXYZRGB>::Ptr keypoints_src_xyzrgb(new pcl::PointCloud <pcl::PointXYZRGB>);
    pcl::PointCloud <pcl::PointXYZRGB>::Ptr keypoints_tgt_xyzrgb(new pcl::PointCloud <pcl::PointXYZRGB>);
    pcl::copyPointCloud(*keypoints_src, *keypoints_src_xyzrgb);
    pcl::copyPointCloud(*keypoints_tgt, *keypoints_tgt_xyzrgb);


    // RandomSampleConsensus bad correspondence rejector
    pcl::registration::CorrespondenceRejectorSampleConsensus <pcl::PointXYZRGB> correspondence_rejector;
    correspondence_rejector.setInputSource (keypoints_src_xyzrgb);
    correspondence_rejector.setInputTarget (keypoints_tgt_xyzrgb);
    correspondence_rejector.setInlierThreshold(RANSAC_Inlier_Threshold);
    correspondence_rejector.setMaximumIterations(RANSAC_Iterations);
    correspondence_rejector.setRefineModel(true);//false
    correspondence_rejector.setInputCorrespondences(all_correspondences);
    correspondence_rejector.getCorrespondences(remaining_correspondences);
}


void compute_Initial_Transformation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &src,
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr &tgt,
    Eigen::Matrix4f &transform,
    pcl::PointCloud<pcl::PointXYZ>::Ptr & keypoints_src_visualize_temp,
    pcl::PointCloud<pcl::PointXYZ>::Ptr & keypoints_tgt_visualize_temp,
    pcl::Correspondences & good_correspondences) {


    // ESTIMATING KEY POINTS
    pcl::PointCloud<pcl::PointWithScale>::Ptr keypoints_src(new pcl::PointCloud<pcl::PointWithScale>);
    pcl::PointCloud<pcl::PointWithScale>::Ptr keypoints_tgt(new pcl::PointCloud<pcl::PointWithScale>);
    detect_keypoints(src, keypoints_src);
    cout << "No of SIFT points in the src are " << keypoints_src->points.size() << endl;

    detect_keypoints(tgt, keypoints_tgt);
    cout << "No of SIFT points in the tgt are " << keypoints_tgt->points.size() << endl;

    // ESTIMATING PFH FEATURE DESCRIPTORS AT KEYPOINTS AFTER COMPUTING NORMALS
    pcl::PointCloud <pcl::Normal>::Ptr src_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud <pcl::Normal>::Ptr tgt_normals(new pcl::PointCloud<pcl::Normal>);

    compute_normals(src, src_normals);
    compute_normals(tgt, tgt_normals);

    // PFHRGB Estimation
    pcl::PointCloud <pcl::PFHRGBSignature250>::Ptr fpfhs_src(new pcl::PointCloud<pcl::PFHRGBSignature250>);;
    pcl::PointCloud <pcl::PFHRGBSignature250>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::PFHRGBSignature250>);;

    compute_PFHRGB_features(src, src_normals, keypoints_src, fpfhs_src);
    compute_PFHRGB_features(tgt, tgt_normals, keypoints_tgt, fpfhs_tgt);
    cout << "End of compute_FPFH_features! " << endl;

    // Copying the pointwithscale to pointxyz so as visualize the cloud
    pcl::copyPointCloud(*keypoints_src, *keypoints_src_visualize_temp);
    pcl::copyPointCloud(*keypoints_tgt, *keypoints_tgt_visualize_temp);

    cout << "SIFT points in the keypoints_src_visualize_temp are " << keypoints_src_visualize_temp->points.size() << endl;
    cout << "SIFT points in the keypoints_tgt_visualize_temp are " << keypoints_tgt_visualize_temp->points.size() << endl;


    // Find correspondences between keypoints in FPFH space
    pcl::CorrespondencesPtr all_correspondences(new pcl::Correspondences);
    findCorrespondences_PFHRGB(fpfhs_src, fpfhs_tgt, *all_correspondences);

    cout << "End of findCorrespondences! " << endl;
    cout << "All correspondences size: " << all_correspondences->size() << endl;

    // Reject correspondences based on their XYZ distance
    /*rejectBadCorrespondences(all_correspondences, keypoints_src_visualize_temp, keypoints_tgt_visualize_temp, good_correspondences);*/
    rejectBadCorrespondences(all_correspondences, keypoints_src, keypoints_tgt, good_correspondences);
    cout << "End of rejectBadCorrespondences! " << endl;
    cout << "Good correspondences size: " << good_correspondences.size() << endl;

    pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
    trans_est.estimateRigidTransformation(*keypoints_src_visualize_temp, *keypoints_tgt_visualize_temp, good_correspondences, transform);
}


int main(int argc, char** argv) {

    // Time start (main function)
    time_t start_computation, end_computation, start_total, end_total;
    time(&start_total);
    time(&start_computation);

    // READ SOURCE AND TARGET FILES
    string src_file = "Plate_no_change_500000_scaled.pcd";
    string tgt_file = "Plate_change_500000.pcd";

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_original(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt_original(new pcl::PointCloud<pcl::PointXYZRGB>);
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(src_file, *src_original) == -1 || pcl::io::loadPCDFile<pcl::PointXYZRGB>(tgt_file, *tgt_original) == -1)
    {
        PCL_ERROR("Couldn't read src or tgt file");
        return -1;
    }

    cout << "Src points: " << src_original->points.size() << endl;
    cout << "Tgt points: " << tgt_original->points.size() << endl;

    // Create the filtering object
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_decimated(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    sor.setInputCloud(src_original);
    sor.setLeafSize(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE);
    sor.filter(*src_decimated);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt_decimated(new pcl::PointCloud<pcl::PointXYZRGB>);
    sor.setInputCloud(tgt_original);
    sor.setLeafSize(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE);
    sor.filter(*tgt_decimated);

    cerr << "Src PointCloud after decimation: " << src_decimated->width * src_decimated->height
        << " data points (" << pcl::getFieldsList(*src_decimated) << ")." << endl;

    cerr << "Tgt PointCloud after decimation: " << tgt_decimated->width * tgt_decimated->height
        << " data points (" << pcl::getFieldsList(*tgt_decimated) << ")." << endl;

    // Filtered point cloud copy
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr src(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZRGB>);
    src = src_decimated;
    tgt = tgt_decimated;

    // Compute the best transformtion
    // Copying the pointwithscale to pointxyz so as visualize the cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_src_visualize_temp(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_tgt_visualize_temp(new pcl::PointCloud<pcl::PointXYZ>);

    // Find correspondences between keypoints in FPFH space
    pcl::CorrespondencesPtr good_correspondences(new pcl::Correspondences);

    // Obtain the initial transformation matirx by using the key-points
    Eigen::Matrix4f transform;
    compute_Initial_Transformation(src, tgt, transform, keypoints_src_visualize_temp, keypoints_tgt_visualize_temp, *good_correspondences);
    cout << "Initial Transformation Matrix" << endl;
    std::cout << transform << std::endl;

    // Time end (main computation)
    time(&end_computation);
    double time_elapsed_computation = difftime(end_computation, start_computation);
    cout << "Elasped computation time in seconds: " << time_elapsed_computation << endl;

// Visualization of keypoints along with the original cloud and correspondences
    pcl::visualization::PCLVisualizer corresp_viewer("Correspondences Viewer");
    corresp_viewer.setBackgroundColor(0, 0, 0);
    corresp_viewer.addPointCloud(src, "Src cloud");
    corresp_viewer.addPointCloud(tgt, "Tgt cloud");
    corresp_viewer.addPointCloud<pcl::PointXYZ>(keypoints_src_visualize_temp, keypoints_src_color_handler, "keypoints_src_corresp_viewer");
    corresp_viewer.addPointCloud<pcl::PointXYZ>(keypoints_tgt_visualize_temp, keypoints_tgt_color_handler, "keypoints_tgt_corresp_viewer");
    corresp_viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints_src_corresp_viewer");
    corresp_viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints_tgt_corresp_viewer");


    for (int i = 0; i < good_correspondences->size(); ++i)
    {
        pcl::PointXYZ & src_idx = keypoints_src_visualize_temp->points[(*good_correspondences)[i].index_query];
        pcl::PointXYZ & tgt_idx = keypoints_tgt_visualize_temp->points[(*good_correspondences)[i].index_match];
        string lineID = to_string(i);
        string lineID2 = to_string(i+200);

        // Generate a random (bright) color
        double r = (rand() % 100);
        double g = (rand() % 100);
        double b = (rand() % 100);
        double max_channel = max(r, max(g, b));
        r /= max_channel;
        g /= max_channel;
        b /= max_channel;

        corresp_viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(src_idx, tgt_idx, r, g, b, lineID);
    }

while (!corresp_viewer.wasStopped()) {
        corresp_viewer.spinOnce();
    }
    corresp_viewer.close();

    // Time end (main function)
    time(&end_total);
    double time_elapsed_total = difftime(end_total, start_total);
    cout << "Elasped total main function time in seconds: " << time_elapsed_total << endl;
}

Possible Solution

Not found yet.

[jasjuang]

Can you enclose your code with triple "`"? It will be easier for us to read that way.

[preethamam]

Sure, I will update it in few minutes.

Sent from my iPhone

On Jul 30, 2019, at 2:15 PM, Jason Juang [email protected] wrote:

Can you enclose your code with triple "`"? It will be easier for us to read that way.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub, or mute the thread.

[preethamam]

Code is readable and highlighted.

[taketwo]

Do I understand right that the correspondences are correct and we can narrow down the problem to visualization?

[preethamam]

Yes, the correspondences look fine. I suspect it is the visualization issue, somehow the point cloud is shifted in the visualization window(s).

[Evan0327]

Hello, is the problem already solved?

[preethamam]

Hello:

The problem is not solved, same issue as the shown pictures.

Thanks,

On Aug 12, 2019, at 5:29 AM, Evan0327 [email protected] wrote:

Hello, is the problem already solved?

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/PointCloudLibrary/pcl/issues/3261?email_source=notifications&email_token=AG2DWTS67PDBCRCUUHO4VDDQEFJSHA5CNFSM4IGY54O2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD4CL4GA#issuecomment-520404504, or mute the thread https://github.com/notifications/unsubscribe-auth/AG2DWTTGXGAUH4GQVX2QNLTQEFJSHANCNFSM4IGY54OQ.

[preethamam]

Does anyone have a suggestion why this is happening?

[stale[bot]]

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

[notu97]

Has this been Solved??? I am facing the same issue.

[preethamam]

I haven't tried the new version of PCL. If you have tried with the new version, then the problem is still persistent and unresolved. PCL administrators need to look into this issue.

[ChaoyiZh]

I have tried this visualization based on the same program and the latest version of PCL; however, the problem are still there .