Table of contents
1. Algorithm principle
1. Overview of the paper
3D laser scanning technology can provide high-precision and large-scale point cloud data for real scene measurement and reconstruction. However, the point cloud from a single source is not complete and fine enough, so using point cloud registration technology to unify point clouds into the same coordinate system has become a basic and critical task in point cloud processing. In the feature-level point cloud registration method, the points with the same name obtained through feature matching often contain extremely high mis-match (gross error) rate, and the current gross error elimination algorithm still has shortcomings in efficiency, precision and recall rate.
The paper describes the 6DOF reduction process in the point cloud registration problem in a simple and intuitive way. And propose a wrong match elimination method based on graph reliability. First, construct a Correspondence Graph (Correspondence Graph) based on the same-named point set obtained by feature matching, and propose the concept of the reliability of the same-named node in the matching graph and measure it with the adjacency matrix of the graph. The optimal candidate subset; similarly use the edge-node affinity matrix to measure the reliability of the same-named edge of the matching graph under the same constraint function, and design a loose constraint function to speed up the calculation of the same-name edge reliability of the tight constraint function and obtain the global maximum Set of interior points, where 6-DOF registration parameter estimation for point-by-point alignment is achieved via a compact constraint function.
2. Implementation process
3. References
[1] Yan L , Wei P , Xie H ,et al.A New Outlier Removal Strategy Based on Reliability of Correspondence Graph for Fast Point Cloud Registration[J]. 2022. DOI:10.1109/TPAMI.2022.3226498.
2. Code implementation
gror_common.h
#pragma once
//windows
#include <chrono>
#include <thread>
#include <vector>
#include <numeric>
//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/icp.h>
#include <pcl/common/geometry.h>
#include <pcl/filters/filter.h>
#include <pcl/features/3dsc.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/flann_search.h>
#include <pcl/search/pcl_search.h>
//Eigen
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
gror_pre.h
/**=====================================================================================================
* Copyright 2020, SCHOOL OF GEODESY AND GEOMATIC, WUHAN UNIVERSITY
* WUHAN, CHINA
* All Rights Reserved
* Authors: Pengcheng Wei, Jicheng Dai, et al.
* Do not hesitate to contact the authors if you have any question or find any bugs
* Email: [email protected]
* See LICENSE for the license information
//=======================================================================================================
* Thanks to the work of Cai, et al:
* https://github.com/ZhipengCai/Demo---Practical-optimal-registration-of-terrestrial-LiDAR-scan-pairs
*/
#pragma once
#ifndef GRORPRE_H_
#define GRORPRE_H_
//windows
#include <thread>
#include <chrono>
#include <iostream>
#include <vector>
//PCL
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/pcl_search.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/conversions.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace GrorPre {
//voxel grid filter
void voxelGridFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVG,
double inlTh);
//ISS keypoint extraction
void issKeyPointExtration(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr iss,
pcl::PointIndicesPtr iss_Idx, double resolution);
//FPFH computation
void fpfhComputation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double resolution, pcl::PointIndicesPtr iss_Idx, pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_out);
//correspondence computation
void correspondenceSearching(pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs,
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfht,
pcl::Correspondences &corr,
int max_corr, std::vector<int> &corr_NOs, std::vector<int> &corr_NOt);
void grorPreparation(pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr issS, pcl::PointCloud<pcl::PointXYZ>::Ptr issT, pcl::CorrespondencesPtr corr, double resolution);
void grorPreparation(pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr issS, pcl::PointCloud<pcl::PointXYZ>::Ptr issT, Eigen::Vector3f ¢erS, Eigen::Vector3f ¢erT, pcl::CorrespondencesPtr corr, double resolution);
void centroidTransMatCompute(Eigen::Matrix4f &T, const Eigen::Vector3f &vS, const Eigen::Vector3f &vT);
}
#endif
ia_gror.h
#pragma once
/**=====================================================================================================
* Copyright 2020, SCHOOL OF GEODESY AND GEOMATIC, WUHAN UNIVERSITY
* WUHAN, CHINA
* All Rights Reserved
* Authors: Pengcheng Wei, Jicheng Dai, et al.
* Do not hesitate to contact the authors if you have any question or find any bugs
* Email: [email protected]
* See LICENSE for the license information
//=======================================================================================================
* Thanks to the work of Cai, et al:
* https://github.com/ZhipengCai/Demo---Practical-optimal-registration-of-terrestrial-LiDAR-scan-pairs
*/
#include <pcl/common/common.h>
#include <pcl/common/geometry.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/matching_candidate.h>
#define DUMMY_PRECISION 1e-12
namespace pcl {
namespace registration
{
template <typename PointSource, typename PointTarget, typename Scalar>
class GRORInitialAlignment : public Registration<PointSource, PointTarget>
{
public:
using Registration<PointSource, PointTarget>::reg_name_;
using Registration<PointSource, PointTarget>::input_;
using Registration<PointSource, PointTarget>::indices_;
using Registration<PointSource, PointTarget>::target_;
using Registration<PointSource, PointTarget>::final_transformation_;
using Registration<PointSource, PointTarget>::transformation_;
using Registration<PointSource, PointTarget>::corr_dist_threshold_;
using Registration<PointSource, PointTarget>::min_number_correspondences_;
using Registration<PointSource, PointTarget>::tree_;
using Registration<PointSource, PointTarget>::transformation_estimation_;
using Registration<PointSource, PointTarget>::getClassName;
typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef boost::shared_ptr<GRORInitialAlignment<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr<const GRORInitialAlignment<PointSource, PointTarget, Scalar> > ConstPtr;
/** \Rotation Element including rotation axis and begin point of axis vector*/
struct RotationElement{
/** \rotation axis. */
Eigen::Vector3f rot_axis;
/** \origin point of rotation. */
Eigen::Vector3f rot_origin;
};
/** \brief intersection interval of a circular arc and an epsilon-ball, used for doing sweep algorithm*/
struct IntervalEnd{
/** \location of the end point. */
double location;
/** \is this end point a starting point of an interval. */
bool is_start;
/** \correspondence index. */
int corr_idx;
void formIntervalEnd(const double &location_in, const bool &is_start_in, const int &corr_idx_in) {
location = location_in;
is_start = is_start_in;
corr_idx = corr_idx_in;
}
};
typedef typename KdTreeFLANN<Scalar>::Ptr FeatureKdTreePtr;
/** \brief Constructor. */
GRORInitialAlignment() : best_count_(3)
{
reg_name_ = "GRORInitialAlignment";
key_source_ = PointCloudSource().makeShared();
key_target_ = PointCloudSource().makeShared();
};
/** \brief set the down sample size (resolution of point cloud)
* \param The resolution of point cloud
*/
void
setResolution(float resolution) {
resolution_ = resolution;
};
/** \brief Get the resolution of point cloud, as set by the user
*/
float
getResolution() {
return resolution_;
};
/** \brief Set the number of used threads if OpenMP is activated.
* \param[in] nr_threads the number of used threads
*/
inline void
setNumberOfThreads(int nr_threads)
{
nr_threads_ = nr_threads;
};
/** \return the number of threads used if OpenMP is activated.
*/
inline int
getNumberOfThreads() const
{
return (nr_threads_);
};
/** \brief set the initial best_cout_ (3 default)
* \param The initial best_cout_
*/
void
setBestCount(int best_count) {
best_count_ = best_count;
};
/** \brief Get the final best count(The number of maximum consistency set) of registration .
*/
int
getBestCount() {
return best_count_;
};
/** \brief Provide a pointer to the vector of the input correspondences.
* \param[in] correspondences the boost shared pointer to a correspondence vector
*/
void
setInputCorrespondences(const CorrespondencesPtr &correspondences);
/** \brief Provide a pointer to the vector of the inlier correspondences.
* \param[in] correspondences the boost shared pointer to a correspondence vector
*/
void
getInlierCorrespondences(CorrespondencesPtr &correspondences) {
correspondences = inliers_; };
/** \brief Set the optimal selection number
* \param n_optimal the optimal selection number
*/
void
setOptimalSelectionNumber(int K_optimal) {
K_optimal_ = K_optimal; };
protected:
/** \brief After the optimal selection step, delete the redundant points and reorganize the correspondence.
* \param[in/out] key_source a cloud of source point
* \param[in/out] key_target a cloud of target point
* \param[in/out] correspondences correspondences points
*/
void
clearReduentPoints(PointCloudSource & key_source, PointCloudTarget & key_target, Correspondences & correspondences);
/** \brief re-build correspondences graph, enumerate all pair of correspondences.
* \param[out] corr_graph Store correspondences graph that meet the geometric constraints.
*/
void
enumeratePairOfCorrespondence(std::vector<std::vector<std::array<Correspondence, 2>>> &corr_graph);
/** \brief Potential unreliable correspondences removal Based On Edge Voting strategy and select fixed N optimal points for Maximum Consistent Set step.
*\param[in] input correspondences
*\param[out] output correspondences
*\param[out] The fixed number of correspondence points for optimal selection.
*/
void
optimalSelectionBasedOnNodeReliability(Correspondences &input_correspondences, Correspondences &output_correspondences, const int K_optimal);
/** \brief obtain the Maximum Consistent Set of correspondences
*\param[in] correspondences a vector store correspondences.
*\param[in] corr_graph correspondences graph information.
*\param[out] two_point_tran_mat a matrix for align two point correspondences.
*\param[out] two_point_rot_ele rotation element for aligned two point correspondences.
*\param[out] best_angle The best rotation angle witch takes two_point_rot_ele.rot_axis as the rotation axis for all correspondences.
*/
void
obtainMaximumConsistentSetBasedOnEdgeReliability(Correspondences & correspondences, std::vector<std::vector<std::array<Correspondence, 2>>> corr_graph, Eigen::Matrix4f &two_point_tran_mat,RotationElement &two_point_rot_ele, float &best_angle);
/** \brief recalculate the transformation matrix by the maximum consistency set.
*\param[out] transform The computed transformation.
*/
Eigen::Matrix4f
refineTransformationMatrix(Eigen::Matrix4f & transform);
/** \brief Rigid transformation computation method.
* \param output the transformed input point cloud dataset using the rigid transformation found
* \param guess The computed transformation
*/
virtual void
computeTransformation(PointCloudSource &output, const Eigen::Matrix4f& guess);
/** \brief The fixed number of correspondence points for optimal selection. */
int K_optimal_;
/** \brief The resolution of point cloud. */
float resolution_;
/** \brief Number of threads for parallelization (standard = 1).
* \note Only used if run compiled with OpenMP.
*/
int nr_threads_;
/** \brief The number of maximum consistency point set.*/
int best_count_;
/** \brief The input correspondences. */
CorrespondencesPtr input_correspondences_;
/** \brief The output correspondences. */
CorrespondencesPtr output_correspondences_;
/** \brief The key point cloud in source after outliers removal. */
PointCloudSourcePtr key_source_;
/** \brief The key point cloud in target after outliers removal. */
PointCloudTargetPtr key_target_;
std::vector<int> remain_source_index_;
std::vector<int> remain_target_index_;
/** \brief The output correspondences. */
CorrespondencesPtr inliers_;
/** \brief The correspondences graph, each elements store a graph construct by a corresponding points and the other corresponding points that meet the geometric constraints after translation.*/
std::vector<std::vector<std::array<Correspondence, 2>>> corr_graph_;
private:
/** \brief two pair correspondences points align.
* \param[in] first_t target point in the first correspondence
* \param[in] first_s source point in the first correspondence
* \param[in] second_t target point in the second correspondence
* \param[in] second_s source point in the second correspondence
* \param[out] rot_element The rotation element for two correspondences
* \return rotation matrix for two pair correspondences points align
*/
Eigen::Matrix4f
twoPairPointsAlign(PointTarget first_t, PointSource first_s, PointTarget second_t, PointSource second_s, RotationElement &rot_element);
/** \brief two vectors align.
* \param[in] a The first vector
* \param[in] b The second vector
* \return Rotation Matrix of two vector
*/
Eigen::Matrix3f
twoVectorsAlign(const Eigen::Vector3f & a, const Eigen::Vector3f & b);
Eigen::Matrix3f
SkewSymmetric(Eigen::Vector3f in);
/** \brief Calculate the Edge Reliability In Relaxed Constraint Function Space(RCFS)
* \param[in] mat rotation matrix for edge pair correspondences align
* \param[in] rotation_element rotation element for two correspondences align
* \param[in] diff_to_s source points after translate to source origin
* \param[in] diff_to_t target points after translate to target origin
* \return degree of the edge reliability in (RCFS)
*/
int
calEdgeReliabilityInRCFS(Eigen::Matrix4f &mat, RotationElement &rotation_element, std::vector<Eigen::Vector3f> &diff_to_s, std::vector<Eigen::Vector3f> &diff_to_t);
/** \brief Calculate the Edge Reliability In Tight Constraint Function Space(RCFS)
* \param[in] transform rotation matrix for two pair correspondences points align
* \param[in] rotation_element rotation element for two pair correspondences points align
* \return 1D rotation angle and max count(lower bound)
*/
std::tuple<float, int>
calEdgeReliabilityInTCFS(Eigen::Matrix4f & transform, RotationElement & rotation_element);
inline
float vl_fast_atan2_f(float y, float x)
{
float angle, r;
float const c3 = 0.1821F;
float const c1 = 0.9675F;
float abs_y = std::abs(y);
if (x >= 0)
{
r = (x - abs_y) / (x + abs_y);
angle = (float)(3.1415926f / 4);
}
else
{
r = (x + abs_y) / (abs_y - x);
angle = (float)(3 * 3.1415926f / 4);
}
angle += (c3*r*r - c1) * r;
return (y < 0) ? -angle : angle;
}
void
removeOutlierPair(CorrespondencesPtr pair_info_ptr, int index);
/** \brief insert angular intervals to a vector.
* \param[out] interval_arr a vector store angular intervals
* \param[in] start_pt start angular(pt) for an interval
* \param[in] start_pt end angular(pt) for an interval
* \param[in] corr_idx correspondence index
*/
void
insertInterval(std::vector<IntervalEnd> &interval_arr, const double &start_pt, const double &end_pt, const int &corr_idx);
/** \brief calculate the circle intersection.
*/
double
circleIntersection(double R, double d, double r);
/** \brief The max-stabbing problem aims to find a vertical line that "stabs" the maximum number of intervals.
* \param[in] interval_array a vector store angular intervals
* \param[out] out_angle stab the out angle
* \param[in] one_to_one is one to one
*/
void
intervalStab(std::vector<IntervalEnd> &interval_array, double &out_angle, int &out_upbnd, bool one_to_one);
};
}
}
#include"ia_gror.hpp"
ia_gror.hpp
#pragma once
#include "ia_gror.h"
#include<omp.h>
bool sortByVoteNumber(const std::pair<int, int> a, const std::pair<int, int> b) {
return a.second > b.second;
};
template <typename T>
bool sortByNumber(const T &a, const T &b) {
return a.size() > b.size();
};
template <class T>
bool compareIntervalEnd(const T &intA, const T &intB)
{
return (intA.location < intB.location);
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::setInputCorrespondences(const CorrespondencesPtr & correspondences)
{
input_correspondences_.reset(new Correspondences);
input_correspondences_ = correspondences;
};
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::clearReduentPoints(PointCloudSource & key_source, PointCloudTarget & key_target, Correspondences & correspondences)
{
std::vector<bool> v_list_t(key_target.size(), false);
std::vector<bool> v_list_s(key_source.size(), false);
PointCloudSource key_target_temp;
PointCloudSource key_source_temp;
for (int i = 0; i<correspondences.size(); ++i) {
v_list_t[correspondences[i].index_match] = true;
v_list_s[correspondences[i].index_query] = true;
}
for (int i = 0; i < v_list_t.size(); ++i) {
if (v_list_t[i]) {
remain_target_index_.push_back(i);
key_target_temp.push_back((*target_)[i]);
}
}
std::map<int, int> look_table;
for (int i = 0; i < remain_target_index_.size(); ++i) {
look_table.insert({
remain_target_index_[i],i });
}
for (int i = 0; i<correspondences.size(); ++i) {
correspondences[i].index_match = look_table[correspondences[i].index_match];
}
look_table.clear();
for (int i = 0; i < v_list_s.size(); ++i) {
if (v_list_s[i]) {
remain_source_index_.push_back(i);
key_source_temp.push_back((*input_)[i]);
}
}
for (int i = 0; i < remain_source_index_.size(); ++i) {
look_table.insert({
remain_source_index_[i],i });
}
for (int i = 0; i < correspondences.size(); ++i) {
correspondences[i].index_query = look_table[correspondences[i].index_query];
}
key_source = key_source_temp;
key_target = key_target_temp;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::enumeratePairOfCorrespondence(std::vector<std::vector<std::array<Correspondence, 2>>> &corr_graph)
{
int corr_size = output_correspondences_->size();
corr_graph.resize(corr_size);
for (int i = 0; i < corr_size; ++i) {
std::vector<std::array<Correspondence, 2>> i_pair_vec;
for (int j = 1; j < corr_size; ++j)
{
if (i >= j) {
continue;
}
auto first_corr = (*output_correspondences_)[i];
auto second_corr = (*output_correspondences_)[j];
auto first_corr_s = (*key_source_)[first_corr.index_query];
auto first_corr_t = (*key_target_)[first_corr.index_match];
auto second_corr_s = (*key_source_)[second_corr.index_query];
auto second_corr_t = (*key_target_)[second_corr.index_match];
float dis_1 = pcl::geometry::distance(first_corr_s, second_corr_s);
float dis_2 = pcl::geometry::distance(first_corr_t, second_corr_t);
float delta_dis = std::abs(dis_1 - dis_2);
if (delta_dis < (2.0*resolution_)/* && dis_1 >(5.0*resolution_)*/)
{
std::array<Correspondence, 2> tmp;
tmp[0] = (*output_correspondences_)[i];
tmp[1] = (*output_correspondences_)[j];
i_pair_vec.emplace_back(tmp);
}
}
corr_graph[i] = i_pair_vec;
}
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::optimalSelectionBasedOnNodeReliability(Correspondences & input_correspondences, Correspondences & output_correspondences, int K_optimal)
{
int corr_size = input_correspondences.size();
std::pair<int, int>init_p(0, 0);
//We directly sum the Adjacent Matrix of each node without list A
std::vector<std::pair<int, int>> node_degree(corr_size, init_p);
//#ifdef _OPENMP
//#pragma omp parallel for shared (node_degree) num_threads(nr_threads_)
//#endif
for (int i = 0; i < corr_size; i++) {
node_degree[i].first = i;
int id_s1 = input_correspondences[i].index_query;
int id_t1 = input_correspondences[i].index_match;
PointSource p_s1 = (*input_).points[id_s1];
PointTarget p_t1 = target_->points[id_t1];
for (int j = 0; j < corr_size; j++) {
if (i >= j) {
continue;
}
int id_s2 = input_correspondences[j].index_query;
int id_t2 = input_correspondences[j].index_match;
PointSource p_s2 = (*input_).points[id_s2];
PointTarget p_t2 = target_->points[id_t2];
float edge_dis_s = pcl::geometry::distance(p_s1, p_s2);
float edge_dis_t = pcl::geometry::distance(p_t1, p_t2);
float delta_dis = std::abs(edge_dis_s - edge_dis_t);
if (delta_dis < 2.0 * resolution_) {
node_degree[i].second++;
node_degree[j].second++;
}
}
}
//Sort points according note number
//Points with higher degree have higher reliability
sort(node_degree.begin(), node_degree.end(), sortByVoteNumber);
Correspondences corr_tmp(K_optimal);
if (corr_size >= K_optimal) {
for (int i = 0; i < K_optimal; i++) {
int id = node_degree[i].first;
corr_tmp[i] = input_correspondences[id];
}
output_correspondences = corr_tmp;
}
else {
output_correspondences = input_correspondences;
}
*key_source_ = *input_;
*key_target_ = *target_;
clearReduentPoints(*key_source_, *key_target_, output_correspondences);
//oblation expriment
//output_correspondences = input_correspondences;
//*key_source_ = *input_;
//*key_target_ = *target_;
}
//each edge pair : E_ij^ST = (e_ij^S, e_ij^T), e_ij^S = (v_i^S, v_j^S), e_ij^T = (v_i^T, v_j^T).
//remain node pairs accompany with each edge pair E_ij^ST :V_k = (v_k^S, v_k^T), k != i != j
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::obtainMaximumConsistentSetBasedOnEdgeReliability(Correspondences & correspondences, std::vector<std::vector<std::array<Correspondence, 2>>> corr_graph, Eigen::Matrix4f &two_point_tran_mat, RotationElement &two_point_rot_ele, float &best_angle)
{
std::sort(corr_graph.begin(), corr_graph.end(), sortByNumber<std::vector<std::array<Correspondence, 2>>>);
//#pragma omp parallel for num_threads(nr_threads_)
for (int i = 0; i < corr_graph.size(); i++) {
if (corr_graph[i].size()<10) {
continue;
}
Correspondence first_pair = corr_graph[i][0][0];//first node pair of the edge pair
auto first_corr_s = (*key_source_)[first_pair.index_query];// first node of the source edge
auto first_corr_t = (*key_target_)[first_pair.index_match];// first node of the target edge
Correspondence second_pair = corr_graph[i][0][1];//first node pair of the edge pair
auto second_corr_s = (*key_source_)[second_pair.index_query];// first node of the source edge
auto second_corr_t = (*key_target_)[second_pair.index_match];// first node of the target edge
std::vector<Eigen::Vector3f> diff_to_s(correspondences.size());
std::vector<Eigen::Vector3f> diff_to_t(correspondences.size());
for (int j = 0; j < correspondences.size(); ++j)
{
auto& t_p = (*key_target_)[correspondences[j].index_match];
auto& s_p = (*key_source_)[correspondences[j].index_query];
diff_to_t[j] = t_p.getVector3fMap() - first_corr_t.getVector3fMap();
diff_to_s[j] = s_p.getVector3fMap() - first_corr_s.getVector3fMap();
}
RotationElement rotation_element;
//align two edge
Eigen::Matrix4f mat = twoPairPointsAlign(first_corr_t, first_corr_s, second_corr_t, second_corr_s, rotation_element);
//degree of edge reliability(der) in relaxed constraint function space(rcfs)
int der_in_rcfs = calEdgeReliabilityInRCFS(mat, rotation_element, diff_to_s, diff_to_t);
if (der_in_rcfs <= best_count_) {
continue;
}
auto result = calEdgeReliabilityInTCFS(mat, rotation_element);
float angle = std::get<0>(result);//last freedom theta
//degree of edge reliability(der) in tight constraint function space(tcfs)
int der_in_tcfs = std::get<1>(result);
if (der_in_tcfs > best_count_)
{
best_count_ = der_in_tcfs;
two_point_rot_ele = rotation_element;
two_point_tran_mat = mat;
best_angle = angle;
}
}
};
template<typename PointSource, typename PointTarget, typename Scalar>
inline Eigen::Matrix4f pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::refineTransformationMatrix(Eigen::Matrix4f & transform)
{
PointCloudSourcePtr gr_issS(new PointCloudSource);
inliers_.reset(new Correspondences);
pcl::transformPointCloud(*input_, *gr_issS, transform);
Eigen::Matrix4f p_p_matrix = Eigen::Matrix4f::Identity();
int sum = 0;
std::vector<int> corr_p_s_index;
std::vector<int> corr_p_t_index;
std::vector<double> v_dists;
for (int i = 0; i < input_correspondences_->size(); i++)
{
int idxS = (*input_correspondences_)[i].index_query;
int idxT = (*input_correspondences_)[i].index_match;
auto t_p = (*target_)[idxT];
auto s_p = (*gr_issS)[idxS];
float dis = pcl::geometry::distance(t_p, s_p);
if (dis < 2 * resolution_)
{
corr_p_s_index.push_back(idxS);
corr_p_t_index.push_back(idxT);
inliers_->push_back((*input_correspondences_)[i]);
v_dists.push_back(dis);
sum++;
}
}
bool use_umeyama_ = true;
if (use_umeyama_)
{
int N = corr_p_t_index.size();
Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src(3, N);
Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt(3, N);
for (int i = 0; i < N; ++i)
{
cloud_src(0, i) = (*input_)[corr_p_s_index[i]].x;
cloud_src(1, i) = (*input_)[corr_p_s_index[i]].y;
cloud_src(2, i) = (*input_)[corr_p_s_index[i]].z;
//++source_it;
cloud_tgt(0, i) = (*target_)[corr_p_t_index[i]].x;
cloud_tgt(1, i) = (*target_)[corr_p_t_index[i]].y;
cloud_tgt(2, i) = (*target_)[corr_p_t_index[i]].z;
//++target_it;
}
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
p_p_matrix = pcl::umeyama(cloud_src, cloud_tgt, false);
}
else {
Eigen::Vector3f mc_t = Eigen::Vector3f::Zero();
Eigen::Vector3f mc_s = Eigen::Vector3f::Zero();
int N = corr_p_t_index.size();
for (int i = 0; i < N; ++i)
{
auto p_t = (*target_)[corr_p_t_index[i]];
auto p_s = (*input_)[corr_p_s_index[i]];
mc_t += Eigen::Vector3f(p_t.x, p_t.y, p_t.z);
mc_s += Eigen::Vector3f(p_s.x, p_s.y, p_s.z);
}
mc_t /= static_cast<double>(N);
mc_s /= static_cast<double>(N);
std::vector<Eigen::Vector3f> pc_t(corr_p_t_index.size());
std::vector<Eigen::Vector3f> pc_s(corr_p_t_index.size());
Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
for (int i = 0; i < N; ++i)
{
auto p_t = (*target_)[corr_p_t_index[i]];
auto p_s = (*input_)[corr_p_s_index[i]];
double weight = 1.0 / (v_dists[i] * 1.0);
pc_t[i] = (Eigen::Vector3f(p_t.x, p_t.y, p_t.z) - mc_t)*weight;
pc_s[i] = (Eigen::Vector3f(p_s.x, p_s.y, p_s.z) - mc_s)*weight;
W += pc_t[i] * (pc_s[i].transpose());
}
Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
Eigen::Matrix3f R = U*(V.transpose());
Eigen::Vector3f t = mc_t - R*mc_s;
p_p_matrix.block<3, 3>(0, 0) = R;
p_p_matrix.block<3, 1>(0, 3) = t;
}
return p_p_matrix;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudSourcePtr input_transformed(new PointCloudSource);
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity())
{
input_transformed->resize(input_->size());
// Apply guessed transformation prior to search for neighbours
pcl::transformPointCloud(*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
output_correspondences_.reset(new Correspondences);
auto t = std::chrono::system_clock::now();
optimalSelectionBasedOnNodeReliability(*input_correspondences_, *output_correspondences_, K_optimal_);
auto t1 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of optimalbSelectionBasedOnNodeReliability: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t).count()) / 1000.0 << std::endl;
enumeratePairOfCorrespondence(corr_graph_);
auto t2 = std::chrono::system_clock::now();
//std::cout << "/*Down!: time consumption of enumeratePairOfCorrespondence: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()) / 1000.0 << std::endl;
Eigen::Matrix4f two_point_tran_mat;
RotationElement two_point_rot_ele;
float best_angle = 0.0;
obtainMaximumConsistentSetBasedOnEdgeReliability(*output_correspondences_, corr_graph_, two_point_tran_mat, two_point_rot_ele, best_angle);
auto t3 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of obtainMaximumConsistentSet: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2).count()) / 1000.0 << std::endl;
Eigen::Matrix4f IdM_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f IdM_2 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f IdM_3 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot = Eigen::AngleAxisf(best_angle, two_point_rot_ele.rot_axis).toRotationMatrix();
IdM_1.block<3, 1>(0, 3) = -1.0*two_point_rot_ele.rot_origin;
IdM_2.block<3, 3>(0, 0) = rot;
IdM_3.block<3, 1>(0, 3) = two_point_rot_ele.rot_origin;
Eigen::Matrix4f gr_tran_mat = IdM_3*IdM_2*IdM_1*two_point_tran_mat;
final_transformation_ = refineTransformationMatrix(gr_tran_mat);
}
//https://math.stackexchange.com/questions/180418/
template<typename PointSource, typename PointTarget, typename Scalar>
inline Eigen::Matrix4f pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::twoPairPointsAlign(PointTarget first_t, PointSource first_s, PointTarget second_t, PointSource second_s, RotationElement &rot_element)
{
Eigen::Vector3f vec_first_t(first_t.x, first_t.y, first_t.z), vec_first_s(first_s.x, first_s.y, first_s.z),
vec_second_t(second_t.x, second_t.y, second_t.z), vec_second_s(second_s.x, second_s.y, second_s.z);
Eigen::Vector3f vec_sour = (vec_first_s - vec_second_s).normalized();
Eigen::Vector3f vec_tart = (vec_first_t - vec_second_t).normalized();
rot_element.rot_axis = vec_tart;
Eigen::Matrix3f rotation_mat = twoVectorsAlign(vec_sour, vec_tart);
Eigen::Matrix4f trans_mat = Eigen::Matrix4f::Identity();
trans_mat.block<3, 3>(0, 0) = rotation_mat;
Eigen::Vector3f translation_v1 = vec_first_t - rotation_mat * vec_first_s;
Eigen::Vector3f translation_v2 = vec_second_t - rotation_mat * vec_second_s;
trans_mat.block<3, 1>(0, 3) = 0.5*(translation_v1 + translation_v2);
rot_element.rot_origin = vec_first_t;
return trans_mat.cast<float>();
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline Eigen::Matrix3f pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::twoVectorsAlign(const Eigen::Vector3f & a, const Eigen::Vector3f & b)
{
Eigen::Vector3f v = a.cross(b);
float s = v.norm();
float c = a.dot(b);
Eigen::Matrix3f rotation_mat = Eigen::Matrix3f::Identity();
Eigen::Matrix3f v_skew = SkewSymmetric(v);
rotation_mat = rotation_mat + v_skew + v_skew*v_skew*(1.0f / (1.0f + c));
return rotation_mat;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline Eigen::Matrix3f pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::SkewSymmetric(Eigen::Vector3f in)
{
Eigen::Matrix3f Skew = Eigen::Matrix3f::Zero();
Skew(0, 1) = -1.0f * in[2];
Skew(0, 2) = in[1];
Skew(1, 0) = in[2];
Skew(1, 2) = -1.0f * in[0];
Skew(2, 0) = -1.0f * in[1];
Skew(2, 1) = in[0];
return Skew;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline int pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::calEdgeReliabilityInRCFS(Eigen::Matrix4f & mat, RotationElement & rotation_element, std::vector<Eigen::Vector3f>& diff_to_s, std::vector<Eigen::Vector3f>& diff_to_t)
{
Eigen::Transform<float, 3, Eigen::Affine> transform = Eigen::Transform<float, 3, Eigen::Affine>(mat);
int sum = 0;//We directly sum the edge-node Affinity Matrix without list M
auto &rot_origin = rotation_element.rot_origin;
auto &rot_axis_t = rotation_element.rot_axis;
Eigen::Vector3f rot_axis_s = transform.rotation().inverse()*rot_axis_t;
//#ifdef _OPENMP
//#pragma omp parallel for reduction(+:sum) num_threads(nr_threads_)
//#endif
for (int i = 0; i < output_correspondences_->size(); i++)
{
Eigen::Vector3f& delta_z_t = diff_to_t[i];
Eigen::Vector3f& delta_z_s = diff_to_s[i];
float dis_t = (delta_z_t).norm();
float dis_s = (delta_z_s).norm();
if (std::abs(dis_t - dis_s) < 2 * resolution_
&&std::abs(delta_z_t.dot(rot_axis_t) - delta_z_s.dot(rot_axis_s)) < 2 * resolution_)
{
sum++;
}
}
return sum;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::removeOutlierPair(CorrespondencesPtr pair_info_ptr, int index)
{
auto iter = pair_info_ptr->begin();
pair_info_ptr->erase(iter + index);
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::insertInterval(std::vector<IntervalEnd>& interval_arr, const double & start_pt, const double & end_pt, const int & corr_idx)
{
IntervalEnd int_end_tmp;
int_end_tmp.formIntervalEnd(start_pt, true, corr_idx);
interval_arr.push_back(int_end_tmp);
int_end_tmp.formIntervalEnd(end_pt, false, corr_idx);
interval_arr.push_back(int_end_tmp);
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline double pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::circleIntersection(double R, double d, double r)
{
assert(R >= 0 && d >= 0 && r >= 0 && "parameters must be positive");
//assert(d<(R+r));
// Return value is between 0 and pi.
double rat, x, angle;
if (d <= DUMMY_PRECISION)
{
return M_PI;
}
// if( fabs(d-(R+r))<DUMMY_PRECISION )
// {
// return 0;
// }
x = (d*d - r*r + R*R) / (2 * d);
rat = x / R;
if (rat <= -1.0)
{
return M_PI;
}
angle = acos(rat);
assert(angle <= M_PI && "angle must be < PI");
return angle;
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline void pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::intervalStab(std::vector<IntervalEnd>& interval_array, double & out_angle, int & out_upbnd, bool one_to_one)
{
std::vector<int> ACTab(key_source_->size(), 0);
int curr_upbnd = 0;
out_upbnd = 0;
//1. sort interval_array
std::sort(interval_array.begin(), interval_array.end(), compareIntervalEnd<IntervalEnd>);
double currLoc = 0;
int NOEnd = 0;
if (!one_to_one) {
for (size_t i = 0; i < interval_array.size(); i++) {
//is a starting point
if (interval_array[i].is_start) {
ACTab[interval_array[i].corr_idx]++;
if (ACTab[interval_array[i].corr_idx] == 1) {
curr_upbnd++;
if (curr_upbnd > out_upbnd) {
out_upbnd = curr_upbnd;
out_angle = interval_array[i].location;
}
}
}
else {
ACTab[interval_array[i].corr_idx]--;
if (ACTab[interval_array[i].corr_idx] == 0) {
NOEnd++;
}
}
if (interval_array[i].location > currLoc) {
curr_upbnd -= NOEnd;
NOEnd = 0;
if (currLoc == out_angle) {
out_angle = (currLoc + interval_array[i].location) / 2;
}
currLoc = interval_array[i].location;
}
}
curr_upbnd -= NOEnd;
}
else {
for (size_t i = 0; i < interval_array.size(); i++) {
//is a starting point
if (interval_array[i].is_start) {
ACTab[interval_array[i].corr_idx]++;
curr_upbnd++;
if (curr_upbnd > out_upbnd) {
out_upbnd = curr_upbnd;
out_angle = interval_array[i].location;
}
}
else {
ACTab[interval_array[i].corr_idx]--;
NOEnd++;
}
if (interval_array[i].location > currLoc) {
curr_upbnd -= NOEnd;
NOEnd = 0;
currLoc = interval_array[i].location;
}
}
curr_upbnd -= NOEnd;
}
}
template<typename PointSource, typename PointTarget, typename Scalar>
inline std::tuple<float, int> pcl::registration::GRORInitialAlignment<PointSource, PointTarget, Scalar>::calEdgeReliabilityInTCFS(Eigen::Matrix4f & transform, RotationElement & rotation_element)
{
std::vector<IntervalEnd> interval_array;
const auto axis = rotation_element.rot_axis;
const auto origin = rotation_element.rot_origin;
PointCloudSourcePtr source_local(new PointCloudSource);
PointCloudTargetPtr target_local(new PointCloudTarget);
Eigen::Matrix4f IdM_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f IdM_2 = Eigen::Matrix4f::Identity();
Eigen::Vector3f z_axis = Eigen::Vector3f::UnitZ();
IdM_1.block<3, 1>(0, 3) = -1.0 * origin;
IdM_2.block<3, 3>(0, 0) = twoVectorsAlign(axis, z_axis);
Eigen::Matrix4f local_tm_t = IdM_2*IdM_1;
Eigen::Matrix4f local_tm_s = local_tm_t*transform.matrix();
pcl::transformPointCloud(*key_source_, *source_local, local_tm_s);
pcl::transformPointCloud(*key_target_, *target_local, local_tm_t);
const size_t msize_total = source_local->size();
const size_t bsize_total = target_local->size();
// Norm of points on XY-plane.
std::vector<float> M_len(msize_total);
std::vector<float> B_len(bsize_total);
// Z-coordinate of points.
std::vector<float> M_z(msize_total);
std::vector<float> B_z(bsize_total);
// Azimuth of points.
std::vector<float> M_azi(msize_total);
std::vector<float> B_azi(bsize_total);
for (int idx = 0; idx < msize_total; ++idx)
{
pcl::PointXYZ s_p = (*source_local)[idx];
float p_x = s_p.x;
float p_y = s_p.y;
M_z[idx] = s_p.z;
M_len[idx] = std::sqrt(p_x*p_x + p_y*p_y);
M_azi[idx] = vl_fast_atan2_f(p_y, p_x);
}
for (int idx = 0; idx < bsize_total; ++idx)
{
pcl::PointXYZ t_p = (*target_local)[idx];
float p_x = t_p.x;
float p_y = t_p.y;
B_z[idx] = t_p.z;
B_len[idx] = std::sqrt(p_x*p_x + p_y*p_y);
B_azi[idx] = vl_fast_atan2_f(p_y, p_x);
}
double dz, d, thMz, rth, dev, beg, end;
double threshold_ = 2.0*resolution_;
for (int i = 0; i < output_correspondences_->size(); i++)
{
int idxS = (*output_correspondences_)[i].index_query;
int idxT = (*output_correspondences_)[i].index_match;
dz = B_z[idxT] - M_z[idxS];
d = B_len[idxT] - M_len[idxS];
thMz = threshold_*threshold_ - dz*dz;
float TWOPI = 2.0*M_PI;
if (d*d <= thMz) {
rth = sqrt(thMz);
//insert the intersection interval to int_idxS
//insert [0,2pi] if M is too short
if (M_len[idxS] <= DUMMY_PRECISION)
{
insertInterval(interval_array, 0, TWOPI, idxS);
}
else
{
dev = circleIntersection(M_len[idxS], B_len[idxT], rth);
if (std::fabs(dev - M_PI) <= DUMMY_PRECISION)
{
/*That could be improved by instead of insert adding 1 to the final quality*/
insertInterval(interval_array, 0, TWOPI, idxS);
}
else
{
beg = std::fmod(B_azi[idxT] - dev - M_azi[idxS], TWOPI);
if (beg < 0)
{
beg += TWOPI;
}
end = std::fmod(B_azi[idxT] + dev - M_azi[idxS], TWOPI);
if (end < 0)
{
end += TWOPI;
}
if (end >= beg)
{
insertInterval(interval_array, beg, end, idxS);
}
else
{
insertInterval(interval_array, beg, TWOPI, idxS);
insertInterval(interval_array, 0, end, idxS);
}
}
}
}
}
double out_angle;
int out_count;
intervalStab(interval_array, out_angle, out_count, true);
return std::make_tuple(out_angle, out_count);
}
gror_pre.cpp
#include "gror_pre.h"
void GrorPre::voxelGridFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVG, double inlTh)
{
//format for filtering
pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2());
pcl::PCLPointCloud2::Ptr cloudVG2(new pcl::PCLPointCloud2());
pcl::toPCLPointCloud2(*cloud, *cloud2);
//set up filtering parameters
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloud2);
sor.setLeafSize(inlTh, inlTh, inlTh);
//filtering process
sor.filter(*cloudVG2);
pcl::fromPCLPointCloud2(*cloudVG2, *cloudVG);
}
void GrorPre::issKeyPointExtration(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr ISS, pcl::PointIndicesPtr ISS_Idx, double resolution)
{
double iss_salient_radius_ = 6 * resolution;
double iss_non_max_radius_ = 4 * resolution;
//double iss_non_max_radius_ = 2 * resolution;//for office
//double iss_non_max_radius_ = 9 * resolution;//for railway
double iss_gamma_21_(0.975);
double iss_gamma_32_(0.975);
double iss_min_neighbors_(4);
int iss_threads_(1); //switch to the number of threads in your cpu for acceleration
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
iss_detector.setSearchMethod(tree);
iss_detector.setSalientRadius(iss_salient_radius_);
iss_detector.setNonMaxRadius(iss_non_max_radius_);
iss_detector.setThreshold21(iss_gamma_21_);
iss_detector.setThreshold32(iss_gamma_32_);
iss_detector.setMinNeighbors(iss_min_neighbors_);
iss_detector.setNumberOfThreads(iss_threads_);
iss_detector.setInputCloud(cloud);
iss_detector.compute(*ISS);
ISS_Idx->indices = iss_detector.getKeypointsIndices()->indices;
ISS_Idx->header = iss_detector.getKeypointsIndices()->header;
}
void GrorPre::fpfhComputation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double resolution, pcl::PointIndicesPtr iss_Idx, pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_out)
{
//compute normal
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setRadiusSearch(3 * resolution);
ne.compute(*normal);
//compute fpfh using normals
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
fpfh_est.setInputCloud(cloud);
fpfh_est.setInputNormals(normal);
fpfh_est.setSearchMethod(tree);
fpfh_est.setRadiusSearch(8 * resolution);
fpfh_est.setNumberOfThreads(16);
fpfh_est.setIndices(iss_Idx);
fpfh_est.compute(*fpfh_out);
}
void GrorPre::correspondenceSearching(pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs, pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfht, pcl::Correspondences & corr, int max_corr, std::vector<int>& corr_NOs, std::vector<int>& corr_NOt)
{
int n = std::min(max_corr, (int)fpfht->size()); //maximum number of correspondences to find for each source point
corr.clear();
corr_NOs.assign(fpfhs->size(), 0);
corr_NOt.assign(fpfht->size(), 0);
// Use a KdTree to search for the nearest matches in feature space
pcl::KdTreeFLANN<pcl::FPFHSignature33> treeS;
treeS.setInputCloud(fpfhs);
pcl::KdTreeFLANN<pcl::FPFHSignature33> treeT;
treeT.setInputCloud(fpfht);
for (size_t i = 0; i < fpfhs->size(); i++) {
std::vector<int> corrIdxTmp(n);
std::vector<float> corrDisTmp(n);
//find the best n matches in target fpfh
treeT.nearestKSearch(*fpfhs, i, n, corrIdxTmp, corrDisTmp);
for (size_t j = 0; j < corrIdxTmp.size(); j++) {
bool removeFlag = true;
int searchIdx = corrIdxTmp[j];
std::vector<int> corrIdxTmpT(n);
std::vector<float> corrDisTmpT(n);
treeS.nearestKSearch(*fpfht, searchIdx, n, corrIdxTmpT, corrDisTmpT);
for (size_t k = 0; k < n; k++) {
if (corrIdxTmpT.data()[k] == i) {
removeFlag = false;
break;
}
}
if (removeFlag == false) {
pcl::Correspondence corrTabTmp;
corrTabTmp.index_query = i;
corrTabTmp.index_match = corrIdxTmp[j];
corrTabTmp.distance = corrDisTmp[j];
corr.push_back(corrTabTmp);
corr_NOs[i]++;
corr_NOt[corrIdxTmp[j]]++;
}
}
}
}
void GrorPre::grorPreparation(pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudT,pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr issS, pcl::PointCloud<pcl::PointXYZ>::Ptr issT, pcl::CorrespondencesPtr corr,double resolution)
{
int max_corr = 5;// neighbor number in descriptor searching
auto t = std::chrono::system_clock::now();
/*=============down sample point cloud by voxel grid filter=================*/
std::cout << "/*voxel grid sampling......" << resolution << std::endl;
GrorPre::voxelGridFilter(origin_cloudS, cloudS, resolution);
GrorPre::voxelGridFilter(origin_cloudT, cloudT, resolution);
auto t1 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of cloud down sample : " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
/*=========================extract iss key points===========================*/
std::cout << "/*extracting ISS keypoints......" << std::endl;
pcl::PointIndicesPtr iss_IdxS(new pcl::PointIndices);
pcl::PointIndicesPtr iss_IdxT(new pcl::PointIndices);
GrorPre::issKeyPointExtration(cloudS, issS, iss_IdxS, resolution);
GrorPre::issKeyPointExtration(cloudT, issT, iss_IdxT, resolution);
auto t2 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of iss key point extraction: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
/*======================fpfh descriptor computation=========================*/
std::cout << "/*fpfh descriptor computation......" << std::endl;
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhS(new pcl::PointCloud<pcl::FPFHSignature33>());
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhT(new pcl::PointCloud<pcl::FPFHSignature33>());
GrorPre::fpfhComputation(cloudS, resolution, iss_IdxS, fpfhS);
GrorPre::fpfhComputation(cloudT, resolution, iss_IdxT, fpfhT);
auto t3 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of fpfh descriptor computation: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2).count()) / 1000.0 << std::endl;
std::cout << "/*size of issS = " << issS->size() << "; size of issT = " << issT->size() << std::endl;
std::cout << "/*=================================================*/" << std::endl;
/*========================correspondences matching=========================*/
std::cout << "/*matching correspondences..." << std::endl;
std::vector<int> corr_NOS, corr_NOT;
GrorPre::correspondenceSearching(fpfhS, fpfhT, *corr, max_corr, corr_NOS, corr_NOT);
auto t4 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of matching correspondences: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t4 - t3).count()) / 1000.0 << std::endl;
std::cout << "/*number of correspondences= " << corr->size() << std::endl;
std::cout << "/*=================================================*/" << std::endl;
}
void GrorPre::grorPreparation(pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT, pcl::PointCloud<pcl::PointXYZ>::Ptr issS, pcl::PointCloud<pcl::PointXYZ>::Ptr issT, Eigen::Vector3f ¢erS, Eigen::Vector3f ¢erT, pcl::CorrespondencesPtr corr, double resolution)
{
int max_corr = 5;// neighbor number in descriptor searching
auto t = std::chrono::system_clock::now();
/*=============down sample point cloud by voxel grid filter=================*/
std::cout << "/*voxel grid sampling......" << resolution << std::endl;
GrorPre::voxelGridFilter(origin_cloudS, cloudS, resolution);
GrorPre::voxelGridFilter(origin_cloudT, cloudT, resolution);
auto t1 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of cloud down sample : " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
/*=========================extract iss key points===========================*/
std::cout << "/*extracting ISS keypoints......" << std::endl;
pcl::PointIndicesPtr iss_IdxS(new pcl::PointIndices);
pcl::PointIndicesPtr iss_IdxT(new pcl::PointIndices);
GrorPre::issKeyPointExtration(cloudS, issS, iss_IdxS, resolution);
GrorPre::issKeyPointExtration(cloudT, issT, iss_IdxT, resolution);
auto t2 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of iss key point extraction: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
//translating the center of both point clouds to the origin
pcl::PointXYZ ps, pt;
pcl::computeCentroid(*issS, ps);
for (int i = 0; i < issS->size(); i++) {
issS->points[i].x -= ps.x;
issS->points[i].y -= ps.y;
issS->points[i].z -= ps.z;
}
pcl::computeCentroid(*issT, pt);
for (int i = 0; i < issT->size(); i++) {
issT->points[i].x -= pt.x;
issT->points[i].y -= pt.y;
issT->points[i].z -= pt.z;
}
centerS =Eigen::Vector3f(ps.x, ps.x, ps.x);
centerT = Eigen::Vector3f(pt.x, pt.x, pt.x);
/*======================fpfh descriptor computation=========================*/
std::cout << "/*fpfh descriptor computation......" << std::endl;
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhS(new pcl::PointCloud<pcl::FPFHSignature33>());
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhT(new pcl::PointCloud<pcl::FPFHSignature33>());
GrorPre::fpfhComputation(cloudS, resolution, iss_IdxS, fpfhS);
GrorPre::fpfhComputation(cloudT, resolution, iss_IdxT, fpfhT);
auto t3 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of fpfh descriptor computation: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2).count()) / 1000.0 << std::endl;
std::cout << "/*size of issS = " << issS->size() << "; size of issT = " << issT->size() << std::endl;
std::cout << "/*=================================================*/" << std::endl;
/*========================correspondences matching=========================*/
std::cout << "/*matching correspondences..." << std::endl;
std::vector<int> corr_NOS, corr_NOT;
GrorPre::correspondenceSearching(fpfhS, fpfhT, *corr, max_corr, corr_NOS, corr_NOT);
auto t4 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of matching correspondences: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t4 - t3).count()) / 1000.0 << std::endl;
std::cout << "/*number of correspondences= " << corr->size() << std::endl;
std::cout << "/*=================================================*/" << std::endl;
}
void GrorPre::centroidTransMatCompute(Eigen::Matrix4f &T, const Eigen::Vector3f &vS, const Eigen::Vector3f &vT){
Eigen::Vector3f t = T.block(0, 3, 3, 1);
Eigen::Matrix3f R = T.block(0, 0, 3, 3);
Eigen::Transform<float, 3, Eigen::Affine> a3f_truth(R);
Eigen::Vector3f centerSt(0,0,0);
pcl::transformPoint(vS, centerSt, a3f_truth);
t = t - vT + centerSt;
T.block(0, 3, 3, 1) = t;
}
main.cpp
/**=====================================================================================================
* Copyright 2020, SCHOOL OF GEODESY AND GEOMATIC, WUHAN UNIVERSITY
* WUHAN, CHINA
* All Rights Reserved
* Authors: Pengcheng Wei, Jicheng Dai, et al.
* Do not hesitate to contact the authors if you have any question or find any bugs
* Email: [email protected]
* See LICENSE for the license information
//=======================================================================================================
*/
//windows
#include <iostream>
#include <thread>
#include <chrono>
#include <complex>
//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/distances.h>
#include "ia_gror.h"
#include "gror_pre.h"
int main(int argc, char** argv)
{
//INPUT:
// 1. path to the source point cloud
std::string fnameS = "E://data//40m1.pcd";
// 2. path to the target point cloud
std::string fnameT = "E://data//40m2.pcd";
// 3. resolution threshold (default 0.1)
double resolution = 0.5;
// 4. optimal threshold (default 800)
int n_optimal = 6;
pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudS(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr origin_cloudT(new pcl::PointCloud<pcl::PointXYZ>);
//support pcd or ply
if (fnameS.substr(fnameS.find_last_of('.') + 1) == "pcd")
{
pcl::io::loadPCDFile(fnameS, *origin_cloudS);
pcl::io::loadPCDFile(fnameT, *origin_cloudT);
}
else if (fnameS.substr(fnameS.find_last_of('.') + 1) == "ply")
{
pcl::io::loadPLYFile(fnameS, *origin_cloudS);
pcl::io::loadPLYFile(fnameT, *origin_cloudT);
}
/*======================================================================*/
auto t = std::chrono::system_clock::now();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr issS(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr issT(new pcl::PointCloud<pcl::PointXYZ>);
pcl::CorrespondencesPtr corr(new pcl::Correspondences);
Eigen::Vector3f centerS(0, 0, 0), centerT(0, 0, 0);
GrorPre::grorPreparation(origin_cloudS, origin_cloudT, cloudS, cloudT, issS, issT, corr, resolution);
auto ShowVGFPointCloud = [](pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> colorT(cloudT, 0, 100, 160);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> colorS(cloudS, 0, 200, 100);
//show point clouds after VGF
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerVGF(new pcl::visualization::PCLVisualizer("After Voxel Grid Downsampling"));
viewerVGF->setBackgroundColor(255, 255, 255);
viewerVGF->addPointCloud<pcl::PointXYZ>(cloudS, colorS, "source cloud");
viewerVGF->addPointCloud<pcl::PointXYZ>(cloudT, colorT, "target cloud");
viewerVGF->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source cloud");
viewerVGF->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
viewerVGF->spin();
};
std::thread vis_thread([cloudS, cloudT, ShowVGFPointCloud]() {
ShowVGFPointCloud(cloudS, cloudT); });
auto t4 = std::chrono::system_clock::now();
pcl::registration::GRORInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, float> gror;
pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);
gror.setInputSource(issS);
gror.setInputTarget(issT);
gror.setResolution(resolution);
gror.setOptimalSelectionNumber(n_optimal);
gror.setNumberOfThreads(1);
gror.setInputCorrespondences(corr);
gror.align(*pcs);
auto t5 = std::chrono::system_clock::now();
std::cout << "/*Down!: time consumption of gror: " << double(std::chrono::duration_cast<std::chrono::milliseconds>(t5 - t4).count()) / 1000.0 << std::endl;
std::cout << "best count: " << gror.getBestCount() << std::endl;
std::cout << "best final TM: \n" << gror.getFinalTransformation() << std::endl;
std::cout << "/*=================================================*/" << std::endl;
auto t_end = std::chrono::system_clock::now();
std::cout << "/*total registration time cost:" << double(std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
pcl::CorrespondencesPtr est_inliers;
pcl::CorrespondencesPtr recall_inliers(new pcl::Correspondences);
pcl::CorrespondencesPtr recall_outliers(new pcl::Correspondences);
pcl::PointCloud<pcl::PointXYZ>::Ptr reg_cloud_S(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloudS, *reg_cloud_S, gror.getFinalTransformation());
bool is_icp = 0;
if (is_icp)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloudS);
icp.setInputTarget(cloudT);
//icp.setMaxCorrespondenceDistance(0.1);
//icp.setTransformationEpsilon(1e-5);
//icp.setEuclideanFitnessEpsilon(0.1);
//icp.setMaximumIterations(100);
icp.setUseReciprocalCorrespondences(true);
icp.align(*reg_cloud_S, gror.getFinalTransformation());
std::cout << "transformation matrix after ICP: \n" << icp.getFinalTransformation() << std::endl;
auto t_end = std::chrono::system_clock::now();
std::cout << "/*total registration time -with icp cost:" << double(std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t).count()) / 1000.0 << std::endl;
std::cout << "/*=================================================*/" << std::endl;
}
pcl::io::savePCDFileBinary("reg_cloud_S.pcd", *reg_cloud_S);
auto ShowVGFPointCloud2 = [](pcl::PointCloud<pcl::PointXYZ>::Ptr cloudS, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> colorT(cloudT, 0, 100, 160);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> colorS(cloudS, 255, 85, 0);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerVGF(new pcl::visualization::PCLVisualizer("After Registration"));
viewerVGF->setBackgroundColor(255, 255, 255);
viewerVGF->addPointCloud<pcl::PointXYZ>(cloudS, colorS, "source cloud");
viewerVGF->addPointCloud<pcl::PointXYZ>(cloudT, colorT, "target cloud");
viewerVGF->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source cloud");
viewerVGF->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
while (!viewerVGF->wasStopped())
{
viewerVGF->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
};
//ShowVGFPointCloud2(reg_cloud_S, cloudT);
}