PCLSLAM.h

#pragma once
#include <NDEVR/SLAMEngine.h>
#include "DLLInfo.h"
#if NDEVR_PCL_SLAM
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/impl/point_types.hpp>
 
#include <pcl/io/pcd_io.h>
 
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
 
#include <pcl/features/normal_3d.h>
 
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
 
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
 
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/range_image/range_image.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include "Base/Headers/Vertex.hpp"
namespace NDEVR
{
    class DoubleExpFilter {
    public:
        Vertex<3, fltp08> updatePosition(const Vertex<3, fltp08>& sensedPosition);
        Vertex<3, fltp08> getCurrentPosition();
        DoubleExpFilter(Vertex<3, fltp08> initialPosition, double dataSmootingFactor, double trendSmoothingFactor);
    private:
        double kAlpha;
        double kBeta;
        Vector<3, fltp08> kSmoothedValue;
        Vertex<3, fltp08> kTrendValue;
        bool kFirstUpdate;
    };
    
    class SLAMProcessor : public SLAMEngine
    {
    public:
        virtual void addFrame(const SLAMFrame& frame) override;
        virtual void setProperty<NDPO::transform>(Matrix<fltp08>& transform) override;
        virtual Matrix<fltp08> getTransform() const override;
        virtual void reset() override;
    public:
        SLAMProcessor();
        Eigen::Matrix4f m_sensor_transform;
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr, Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZ>::Ptr> > m_frames;
        uint04 m_total_points = 0U;
 
 
    private:
        pcl::PointCloud<pcl::PointXYZ>::Ptr pairAlign(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_src, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_tgt, const SLAMFrame& frame, Eigen::Matrix4f& final_transform);
        void computeNarfKeypoint(pcl::PointCloud<pcl::PointXYZ>::ConstPtr point_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr);
        /* please save */
        void computeSurfaceNormals(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& points, pcl::PointCloud<pcl::Normal>::Ptr& normals);
        void computeLocalFeatures(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& points, const pcl::PointCloud<pcl::Normal>::ConstPtr& normals, pcl::PointCloud<pcl::FPFHSignature33>::Ptr& features);
        pcl::search::KdTree<pcl::PointXYZ>::Ptr search_method_xyz;
 
        DoubleExpFilter* m_filter;
        double lastFrameEndTime;
 
        double param1, param2;
 
    };

    class PCLSLAMEngineFactory : public SLAMEngineFactory
    {
    public:
        static void Register();
        SLAMEngine* createSLAMEngine() final override;
        UUID id() const final override { return UUID::CreateUUID("SLAMProcessor"); };
    };
}
#endif