GICPAlignment.h

Go to the documentation of this file.

#pragma once
#ifndef _GICP_ALIGNMENT_H
#define _GICP_ALIGNMENT_H

#include <Utils.h>
#include <pcl/registration/gicp.h>

class GICPAlignment
{
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
    typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudRGB;
    typedef pcl::PointCloud<pcl::Normal> PointCloudNormal;
    typedef std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> CovariancesVector;

public:
    GICPAlignment(PointCloudRGB::Ptr target_cloud, PointCloudRGB::Ptr source_cloud, bool use_covariances);

    ~GICPAlignment(){};

    bool transform_exists_;


    void run();

    void iterate();

    void undo();

    Eigen::Matrix4f getFineTransform();

    void getAlignedCloud(PointCloudRGB::Ptr aligned_cloud);

    void getAlignedCloudROSMsg(sensor_msgs::PointCloud2& aligned_cloud_msg);

    void applyTFtoCloud(PointCloudRGB::Ptr cloud);

    void setSourceCloud(PointCloudRGB::Ptr source_cloud);

    void setTargetCloud(PointCloudRGB::Ptr target_cloud);

    void setMaxIterations(int iterations);

    void setTfEpsilon(double tf_epsilon);
    
    void setMaxCorrespondenceDistance(int max_corresp_distance);
    
    void setRANSACOutlierTh(int ransac_threshold);

private:

    bool covariances_;
    
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> gicp_;

    Eigen::Matrix4f fine_tf_;

    
    int max_iter_;

    double tf_epsilon_;

    double max_corresp_distance_;

    double ransac_outlier_th_;

    PointCloudRGB::Ptr target_cloud_;

    PointCloudRGB::Ptr source_cloud_;

    PointCloudRGB::Ptr aligned_cloud_;

    PointCloudRGB::Ptr backup_cloud_;

    void configParameters();

    void fineAlignment();

    void getCovariances(PointCloudRGB::Ptr cloud, boost::shared_ptr<CovariancesVector> covs);

    void applyCovariances();
    
    void iterateFineAlignment(PointCloudRGB::Ptr cloud);

    void backUp(PointCloudRGB::Ptr cloud);  
};

#endif