Find the overlap of two point clouds

Posted by patrick99e99 on Sun, 16 Jan 2022 18:13:52 +0100

Most of the search for overlapping areas on the Internet is to establish a kdtree for one point cloud, and then search the points of another point cloud within the r radius. This method is suitable for two point clouds, which are exactly the same. The general point cloud data are not exactly the same. For example, the point clouds of two flight belts are not exactly the same. If this method is applied, many points will be lost, resulting in the inaccuracy of feature calculation. Two methods are described below:

The conclusion written above: the second method is superior to the first method in speed and accuracy (octree yyds!!!)

Method 1:

(1) Suppose there are two groups of point clouds a and B, calculate the OBB bounding box of A
(2) Find the point cloud B in the OBB bounding box of point cloud A, so as to obtain the overlapping area of Point Cloud B relative to A
(3) On the contrary, the overlapping area of point cloud A relative to B is obtained
(4) To sum up, we can get a group of point clouds in overlapping areas, which can be used for subsequent analysis.
show the codes

// Extract the overlapping area of point cloud cpp: this file contains the "main" function. Program execution will begin and end here.
//

#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include<pcl/filters/crop_box.h>
#include <pcl/common/transforms.h>
#include<pcl/common/common.h>
//Gets the overlapping area of the second point cloud relative to the first point cloud
void getOverlappedCloud(const pcl::PointCloud<pcl::PointXYZ> &cloud, const pcl::PointCloud<pcl::PointXYZ>&cloud2, pcl::PointCloud<pcl::PointXYZ>&overlapped_cloud2)
{
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud.makeShared());
    feature_extractor.compute();
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    feature_extractor.getAABB(min_point_AABB, max_point_AABB);
    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
   //Rotate the point cloud so that the OBB of the first point cloud = AABB, so as to obtain the second point cloud in the OBB area of the first point cloud
    Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
    tf << (rotational_matrix_OBB.inverse());
    pcl::PointCloud<pcl::PointXYZ>cloud_tf, cloud2_tf;
    pcl::transformPointCloud(cloud, cloud_tf, tf);
    pcl::transformPointCloud(cloud2, cloud2_tf, tf);
    Eigen::Vector4f pmin, pmax;

    pcl::getMinMax3D(cloud_tf, pmin, pmax);
    pcl::PointCloud<pcl::PointXYZ> tf_overlapped_cloud2;
    std::vector<int> indices;
    //The point cloud overlap area after the second point cloud rotation
    pcl::getPointsInBox(cloud2_tf, pmin, pmax, indices);
    pcl::copyPointCloud(cloud2_tf, indices, tf_overlapped_cloud2);
    //Then transform to the original coordinate system
    pcl::transformPointCloud(tf_overlapped_cloud2, overlapped_cloud2, tf.inverse());

    std::cout << tf << std::endl;
    std::cout << rotational_matrix_OBB << std::endl;
}
int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>cloud, overlapped_cloud;
    pcl::io::loadPCDFile("SHCSCloud.pcd", cloud);

    pcl::PointCloud<pcl::PointXYZ>cloud2,overlapped_cloud2;
    pcl::io::loadPCDFile("SHCSCloud copy.pcd", cloud2);

    getOverlappedCloud(cloud, cloud2, overlapped_cloud2);
    getOverlappedCloud(cloud2, cloud, overlapped_cloud);

    pcl::io::savePCDFile<pcl::PointXYZ>("Overlapping point cloud.pcd", overlapped_cloud);
    pcl::io::savePCDFile<pcl::PointXYZ>("Overlapping point cloud 2.pcd", overlapped_cloud2);
}

Method 1 experimental effect:

Original point cloud:

The white point cloud in the figure is the overlapping area of the original red point cloud:

The green point cloud is the overlapping area of the blue original point cloud:

Time consuming:

Conclusion: from the graph, this method can only rough the overlapping area of point cloud, and there is a certain deviation between the extracted overlapping area and the actual overlapping area.

Method 2:

(1) Suppose there are two groups of point clouds A and B, and establish an octree for A
(2) Traverse all points in B and query whether the voxel of A corresponding to it has A point cloud (i.e. whether AB point cloud also exists in the same voxel). If so, the point is the overlapping point cloud of B point cloud
(3) Similarly, the overlapping point cloud in A is obtained
show the codes

// 2. Extract the overlapping area of point cloud CPP: this file contains the "main" function. Program execution will begin and end here.
//

#include <iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/octree/octree.h>
#include<ctime>
using point = pcl::PointXYZ;
using cloud = pcl::PointCloud<point>;
void getOverlappedCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud1, const pcl::PointCloud<pcl::PointXYZ>& cloud2, pcl::PointCloud<pcl::PointXYZ>& overlapped_cloud2)
{
    double radius = 3.0;
    pcl::octree::OctreePointCloudSearch<point> octree(radius);
    octree.setInputCloud(cloud1.makeShared());
    octree.addPointsFromInputCloud();

    //pcl::PointCloud<point> overlapped_2;
    for (size_t i = 0; i < cloud2.size(); ++i)
    {
        std::vector<int> indices;
        octree.voxelSearch(cloud2.points[i], indices);
        pcl::PointCloud<point> cloud_out;
        if (indices.size())
        {
            overlapped_cloud2.push_back(cloud2.points[i]);
        }

    }
}
int main()
{
    auto start = std::clock();
    pcl::PointCloud<point> cloud1, cloud2;
    pcl::io::loadPCDFile<point>("SHCSCloud.pcd", cloud1);
    pcl::io::loadPCDFile<point>("SHCSCloud copy.pcd", cloud2);
    cloud overlapped1, overlapped2;
    getOverlappedCloud(cloud1, cloud2, overlapped2);
    getOverlappedCloud(cloud2, cloud1, overlapped1);

    pcl::io::savePCDFile("Overlap 1.pcd", overlapped1);
    pcl::io::savePCDFile("Overlap 2.pcd", overlapped2);
    auto end = std::clock();
    std::cerr << "time consuming" << std::difftime(end, start) << "ms" << std::endl;
    std::cout << "Hello World!\n";
}