#ifndef NDT_SLAM_H
#define NDT_SLAM_H

#include"def.h"
#include"undistort.h"

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>

class CNdtSlam
{

public:
    CNdtSlam();
    ~CNdtSlam();

    bool SaveFrame();
    int  AddImu(ImuData& imu);
    int  AddScan(LidarFrame& scan);

    int  OutputMap(string file);

private:

    CLidarUnDistort mLidarUndistort;
    
     //保存关键帧的位置: (x,y,z)
    pcl::PointCloud<PointXYZI> mKeyFramesPose3D;
    //保存位置和姿态： (x,y,z,r,p,y)
    pcl::PointCloud<PointTypePose> mKeyFramesPose6D;

    //保存关键帧的点云
    std::vector< pcl::PointCloud<PT_TYPE> > mKeyFrames; 

    PointTypePose mCurrentPose;
    double        mCurrentStamp;
    float surroundingkeyframeAddingDistThreshold;
    float surroundingkeyframeAddingAngleThreshold;

    pclomp::NormalDistributionsTransform<PT_TYPE, PT_TYPE> mOmpNdt;
    
};





#endif