InitialAlignment.h

Go to the documentation of this file.

#pragma once
#ifndef _INITIAL_ALIGNMENT_H
#define _INITIAL_ALIGNMENT_H

#include <Utils.h>

namespace AlignmentMethods  
{
    enum AlignmentMethod
    {
      HARRIS,
      BOUNDARY,
      MULTISCALE,
      NORMALS,
      NONE
    };
}
typedef AlignmentMethods::AlignmentMethod AlignmentMethod;

class InitialAlignment
{
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
    typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudRGB;
    typedef pcl::PointCloud<pcl::Normal> PointCloudNormal;
    typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudFPFH;

public:
    InitialAlignment(PointCloudRGB::Ptr target_cloud, PointCloudRGB::Ptr source_cloud);

    ~InitialAlignment(){};

    bool transform_exists_;

    void run();

    Eigen::Matrix4f getRigidTransform();

    void getAlignedCloud(PointCloudRGB::Ptr aligned_cloud);

    void getAlignedCloudROSMsg(sensor_msgs::PointCloud2& aligned_cloud_msg);

    void applyTFtoCloud(PointCloudRGB::Ptr cloud);

    void applyTFtoCloud(PointCloudRGB::Ptr cloud, Eigen::Matrix4f tf);

    void setMethod(AlignmentMethod method);

    void setRadiusFactor(double align_factor);

private:
    Eigen::Matrix4f rigid_tf_;

    double normal_radius_;

    double radius_factor_;

    PointCloudRGB::Ptr target_cloud_;

    PointCloudRGB::Ptr source_cloud_;

    PointCloudRGB::Ptr aligned_cloud_;

    PointCloudNormal::Ptr source_normals_;

    PointCloudNormal::Ptr target_normals_;

    PointCloudNormal::Ptr source_dominant_normals_;

    PointCloudNormal::Ptr target_dominant_normals_;

    PointCloudRGB::Ptr target_keypoints_;
    
    PointCloudRGB::Ptr source_keypoints_;

    PointCloudFPFH::Ptr target_features_;

    PointCloudFPFH::Ptr source_features_;

    pcl::CorrespondencesPtr correspondences_;

    AlignmentMethod method_;

    Eigen::Vector4f source_centroid_;

    Eigen::Vector4f target_centroid_;

    void applyTFtoCloud();

    void runNormalsBasedAlgorithm();

    void runKeypointsBasedAlgorithm();

    void obtainNormalsCluster(PointCloudRGB::Ptr cloud,
                              PointCloudNormal::Ptr normals,
                              std::vector<pcl::PointIndices>& cluster_indices);

    void obtainDominantNormals(PointCloudNormal::Ptr normals,
                              std::vector<pcl::PointIndices> cluster_indices,
                              PointCloudNormal::Ptr dominant_normals);                 

    bool normalsCorrespondences(PointCloudNormal::Ptr source_dominant_normal,
                                PointCloudNormal::Ptr target_dominant_normal,
                                std::vector<int>& source_idx, std::vector<int>& target_idx);

    void getTransformationFromNormals(PointCloudNormal::Ptr source_normals,
                                      PointCloudNormal::Ptr target_normals);

    bool findIdx(Eigen::MatrixXd source_m, Eigen::MatrixXd target_m,
                Eigen::MatrixXi source_idx_m, Eigen::MatrixXi target_idx_m,
                std::vector<int>& source_indx, std::vector<int>& target_indx);

    Eigen::Matrix3f getCoordinateSystem(PointCloudNormal::Ptr normals, 
                                        std::vector<int>& index);

    void matrixFromAngles(PointCloudNormal::Ptr normal,
                          size_t size,
                          Eigen::MatrixXd& matrix,
                          Eigen::MatrixXi& index_matrix);                                                                                                   
    
    Eigen::VectorXd nextVector(Eigen::MatrixXd matrix,
                              int current_col, int current_row);

    int obtainHarrisKeypoints(PointCloudRGB::Ptr cloud,
                              PointCloudNormal::Ptr normals,
                              PointCloudRGB::Ptr keypoints_cloud,
                              pcl::IndicesPtr keypoints_indices);  

    int obtainBoundaryKeypoints(PointCloudRGB::Ptr cloud,
                                PointCloudNormal::Ptr normals,
                                PointCloudRGB::Ptr keypoints_cloud,
                                pcl::IndicesPtr keypoints_indices,
                                pcl::IndicesPtr non_keypoints_indices);

    int obtainMultiScaleKeypointsAndFeatures(PointCloudRGB::Ptr cloud,
                                            PointCloudNormal::Ptr normals,
                                            PointCloudRGB::Ptr keypoints_cloud,
                                            pcl::IndicesPtr keypoints_indices,
                                            PointCloudFPFH::Ptr features);

    int obtainFeatures(PointCloudRGB::Ptr cloud,
                      PointCloudNormal::Ptr normals,
                      pcl::IndicesPtr keypoints_indices,
                      PointCloudFPFH::Ptr features);                                                                                               

    int obtainKeypointsAndFeatures(PointCloudRGB::Ptr cloud, 
                                  PointCloudNormal::Ptr normals,
                                  PointCloudRGB::Ptr keypoints_cloud,
                                  PointCloudFPFH::Ptr features_cloud);

    void obtainCorrespondences();

    void rejectCorrespondences();

    void rejectOneToOneCorrespondences();

    void estimateTransform();                                                                              
};

#endif