lego-loam mapOptmization 源码注释(一)
我们先看主函数:
一、Main()
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();
MO.run();
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
我估计能看到这的朋友,对lego-loam已经很熟悉了,这个main函数,总体上和featureAssociation的main函数一模一样,只是多了四行,我们着重看多出来部分的作用:
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
//这行代码创建了一个名为loopthread的线程对象,它将执行mapOptimization类的loopClosureThread成员函数。&MO是传递给线程的参数,意味着loopClosureThread函数将在MO这个mapOptimization类的实例上被调用。
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
//这行代码创建了另一个名为visualizeMapThread的线程对象,它将执行mapOptimization类的visualizeGlobalMapThread成员函数,同样传递MO作为参数。
loopthread.join();
//这行代码会阻塞当前线程(通常是主线程),直到loopthread线程完成执行。join函数用于等待线程结束,确保在程序继续执行之前,该线程的所有任务都已完成。
visualizeMapThread.join();
//这行代码同样会阻塞当前线程,直到visualizeMapThread线程完成执行。
这两个线程的作用是并行处理地图优化中的两个重要任务,以提高SLAM系统的效率和准确性。闭环检测线程负责提高地图的准确性,而全局地图可视化线程则提供了地图的可视化展示。
这两个线程是独立于主线程的。在C++中,使用std::thread
创建的线程是并发执行的,这意味着它们可以同时运行,而不受主线程的直接控制。每个线程都有自己的执行路径和执行栈。
主线程则负责处理ROS回调、调用MO.run()
执行地图优化的主要逻辑,并通过ros::spinOnce()
与ROS通信。主线程通过调用join()
方法等待这两个线程完成,确保在程序退出前,所有线程都已经正确地结束了它们的任务。
我的想法是先看主线程,然后再看回环检测,最后看可视化的内容。
先来看mapOptimization类!!
mapOptimization
补充一下:之前一直没看到gtsam在lego-loam的作用,看来应该是在地图优化上发挥作用了。
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
下面我们来看mapOptimization的私有变量定义,不详细解释,筛一些有意思的说一下。
private:
class mapOptimization{
private:
NonlinearFactorGraph gtSAMgraph;
//这是一个非线性因子图,用于表示非线性最小二乘问题中的约束。
在SLAM中,这些约束通常来自于传感器测量,如激光雷达、雷达或视觉里程计数据。
每个因子代表一个测量,而每个节点代表一个状态变量(如机器人的位置和方向)。
Values initialEstimate;
//Values是一个键值对集合,用于存储变量的初始估计值。
在SLAM中,这些初始估计可能来自于先验知识、传感器校准或其他来源。
这些值作为优化算法的起点。
Values optimizedEstimate;
//这是一个Values对象,用于存储优化后的估计值。
在执行优化算法后,这个对象将包含更新的状态变量值,这些值最小化了因子图中所有因子的误差。
ISAM2 *isam;
//ISAM2是gtSAM库中的一个增量状态调整和映射(Incremental Smoothing and Mapping)类的实例。
这个类用于在线更新状态估计,同时处理新的测量数据和先前的估计。
ISAM2是一个高效的算法,可以处理大规模的非线性优化问题。
Values isamCurrentEstimate;
//这是一个Values对象,用于存储ISAM2当前的状态估计。
随着新的测量数据不断到来,ISAM2会更新其内部的状态估计,这个对象就用来存储这些最新的估计值。
!!具体的用法要在代码中去看,这里只是有个印象!!
noiseModel::Diagonal::shared_ptr priorNoise;
//这是一个指向noiseModel::Diagonal对象的智能指针,用于表示先验噪声模型。
在SLAM中,先验噪声模型通常用于表示对初始状态估计的不确定性。
例如,在机器人的初始位置和方向估计中,先验噪声模型可以表示为位置和方向的不确定性。
noiseModel::Diagonal::shared_ptr odometryNoise;
//这是一个指向noiseModel::Diagonal对象的智能指针,用于表示里程计噪声模型。
里程计噪声模型用于表示机器人运动测量(如轮式里程计或视觉里程计)的不确定性。
这些测量通常包含噪声,需要通过噪声模型来量化。
noiseModel::Diagonal::shared_ptr constraintNoise;
//这是一个指向noiseModel::Diagonal对象的智能指针,用于表示约束噪声模型。
在SLAM中,约束通常来自于传感器测量,如激光雷达、雷达或视觉里程计数据。
约束噪声模型用于表示这些测量的不确定性。
ros::NodeHandle nh;
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOdomAftMapped;
ros::Publisher pubKeyPoses;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRegisteredCloud;
ros::Subscriber subLaserCloudCornerLast;
ros::Subscriber subLaserCloudSurfLast;
ros::Subscriber subOutlierCloudLast;
ros::Subscriber subLaserOdometry;
ros::Subscriber subImu;
nav_msgs::Odometry odomAftMapped;
tf::StampedTransform aftMappedTrans;
tf::TransformBroadcaster tfBroadcaster;
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> outlierCloudKeyFrames;
deque<pcl::PointCloud<PointType>::Ptr> recentCornerCloudKeyFrames;
deque<pcl::PointCloud<PointType>::Ptr> recentSurfCloudKeyFrames;
deque<pcl::PointCloud<PointType>::Ptr> recentOutlierCloudKeyFrames;
int latestFrameID;
vector<int> surroundingExistingKeyPosesID;
deque<pcl::PointCloud<PointType>::Ptr> surroundingCornerCloudKeyFrames;
deque<pcl::PointCloud<PointType>::Ptr> surroundingSurfCloudKeyFrames;
deque<pcl::PointCloud<PointType>::Ptr> surroundingOutlierCloudKeyFrames;
PointType previousRobotPosPoint;
PointType currentRobotPosPoint;
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses;
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS;
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudOutlierLast; // corner feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDS; // corner feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLast; // surf feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLastDS; // downsampled corner featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloudDS;
pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloudDS;
pcl::PointCloud<PointType>::Ptr latestCornerKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloudDS;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap;
pcl::PointCloud<PointType>::Ptr globalMapKeyPoses;
pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS;
pcl::PointCloud<PointType>::Ptr globalMapKeyFrames;
pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterOutlier;
pcl::VoxelGrid<PointType> downSizeFilterHistoryKeyFrames; // for histor key frames of loop closure
pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
double timeLaserCloudCornerLast;
double timeLaserCloudSurfLast;
double timeLaserOdometry;
double timeLaserCloudOutlierLast;
double timeLastGloalMapPublish;
bool newLaserCloudCornerLast;
bool newLaserCloudSurfLast;
bool newLaserOdometry;
bool newLaserCloudOutlierLast;
float transformLast[6];
float transformSum[6];
float transformIncre[6];
float transformTobeMapped[6];
float transformBefMapped[6];
float transformAftMapped[6];
int imuPointerFront;
int imuPointerLast;
double imuTime[imuQueLength];
float imuRoll[imuQueLength];
float imuPitch[imuQueLength];
std::mutex mtx;
double timeLastProcessing;
PointType pointOri, pointSel, pointProj, coeff;
cv::Mat matA0;
cv::Mat matB0;
cv::Mat matX0;
cv::Mat matA1;
cv::Mat matD1;
cv::Mat matV1;
bool isDegenerate;
cv::Mat matP;
int laserCloudCornerFromMapDSNum;
int laserCloudSurfFromMapDSNum;
int laserCloudCornerLastDSNum;
int laserCloudSurfLastDSNum;
int laserCloudOutlierLastDSNum;
int laserCloudSurfTotalLastDSNum;
bool potentialLoopFlag;
double timeSaveFirstCurrentScanForLoopClosure;
int closestHistoryFrameID;
int latestFrameIDLoopCloure;
bool aLoopIsClosed;
float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ;
float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ;
我们重点关注代码中注释的部分,以及接收和发布的数据!!
ros::NodeHandle nh;
ros::Publisher pubLaserCloudSurround;
//用于发布地图映射后的点云数据
ros::Publisher pubOdomAftMapped;
//用于发布映射后的里程计信息,通常包含机器人的位置和姿态。
ros::Publisher pubKeyPoses;
//用于发布关键帧的姿态信息,这些关键帧可能用于SLAM中的位姿图优化。
ros::Publisher pubHistoryKeyFrames;
//可能用于发布历史关键帧的信息,用于可视化或记录。
ros::Publisher pubIcpKeyFrames;
//用于发布与ICP(迭代最近点)算法相关的关键帧信息。
ros::Publisher pubRecentKeyFrames;
//用于发布最近的关键帧信息,可能用于实时显示或处理。
ros::Publisher pubRegisteredCloud;
//用于发布已配准的点云数据,这通常是将多个传感器数据融合后的结果。
//接收的数据包括角特征、面特征、离群点、经过特征计算过位姿的雷达里程计、imu数据。
ros::Subscriber subLaserCloudCornerLast;
ros::Subscriber subLaserCloudSurfLast;
ros::Subscriber subOutlierCloudLast;
ros::Subscriber subLaserOdometry;
ros::Subscriber subImu;
pubic:
public:
mapOptimization():
nh("~")
{
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 2);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/history_cloud", 2);
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/corrected_cloud", 2);
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/recent_cloud", 2);
pubRegisteredCloud = nh.advertise<sensor_msgs::PointCloud2>("/registered_cloud", 2);
downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterHistoryKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for histor key frames of loop closure
downSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for surrounding key poses of scan-to-map optimization
downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for global map visualization
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
aftMappedTrans.frame_id_ = "camera_init";
aftMappedTrans.child_frame_id_ = "/aft_mapped";
allocateMemory();
}
ISAM2Params
是一个结构或类,用于配置ISAM2(Incremental Smoother and Mapper 2)算法的参数。
-
parameters.relinearizeThreshold = 0.01;
-
relinearizeThreshold参数定义了重新线性化的阈值。在ISAM2中,由于处理的是非线性问题,状态变量的误差可能会变得非常大,导致线性化近似不再有效。当状态变量的误差超过这个阈值时,ISAM2会重新线性化这些变量,以保持优化过程的准确性。设置为0.01意味着当状态变量的误差超过1%时,ISAM2将重新线性化这些变量。
-
parameters.relinearizeSkip = 1;
-
relinearizeSkip参数定义了重新线性化的跳过次数。这个参数控制了在重新线性化阈值被触发后,ISAM2在实际执行重新线性化操作之前会跳过多少个优化迭代。设置为1意味着ISAM2将在每次重新线性化阈值被触发时立即执行重新线性化,而不会跳过任何迭代。
关于几个回调函数没有什么需要特别注意的地方,大家看一下就好了,有问题一起讨论!!
void laserCloudOutlierLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
timeLaserCloudOutlierLast = msg->header.stamp.toSec();
laserCloudOutlierLast->clear();
pcl::fromROSMsg(*msg, *laserCloudOutlierLast);
newLaserCloudOutlierLast = true;
}
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
timeLaserCloudCornerLast = msg->header.stamp.toSec();
laserCloudCornerLast->clear();
pcl::fromROSMsg(*msg, *laserCloudCornerLast);
newLaserCloudCornerLast = true;
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
timeLaserCloudSurfLast = msg->header.stamp.toSec();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*msg, *laserCloudSurfLast);
newLaserCloudSurfLast = true;
}
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){
timeLaserOdometry = laserOdometry->header.stamp.toSec();
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;
newLaserOdometry = true;
}
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
}
几个降采样可以稍微关注一下:
downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterHistoryKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for histor key frames of loop closure
downSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for surrounding key poses of scan-to-map optimization
downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for global map visualization
这段代码是使用PCL(Point Cloud Library,点云库)中的VoxelGrid
滤波器来对点云数据进行降采样。VoxelGrid
滤波器通过将点云中的点分配到一个三维体素网格中,并只保留每个体素(或“叶”)中的一个点(通常是中心点或最近的点),从而减少点云数据的数量。这种方法可以提高处理速度,同时在一定程度上保留点云的形状和结构。
每个setLeafSize
函数调用都设置了滤波器的体素大小,体素大小决定了降采样的程度。参数分别是x、y和z轴上的体素尺寸。体素尺寸越小,降采样后的点云越精细,但计算量也越大;体素尺寸越大,降采样后的点云越粗糙,但计算量越小。
然后我们来看allocateMemory();函数。
allocateMemory()
整体来看,allocateMemory也没有太多可以注释的点,有问题欢迎大家在评论区讨论!!
void allocateMemory(){
cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
//3d代表的是点云,用于存储点云点
cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
//6d代表的是姿态,用于存储变换矩阵
kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
surroundingKeyPoses.reset(new pcl::PointCloud<PointType>());
surroundingKeyPosesDS.reset(new pcl::PointCloud<PointType>());
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
laserCloudOutlierLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
laserCloudOutlierLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner feature set from odoOptimization
laserCloudSurfTotalLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
laserCloudSurfTotalLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
nearHistoryCornerKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
nearHistoryCornerKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());
nearHistorySurfKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
nearHistorySurfKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());
latestCornerKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
latestSurfKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
latestSurfKeyFrameCloudDS.reset(new pcl::PointCloud<PointType>());
kdtreeGlobalMap.reset(new pcl::KdTreeFLANN<PointType>());
globalMapKeyPoses.reset(new pcl::PointCloud<PointType>());
globalMapKeyPosesDS.reset(new pcl::PointCloud<PointType>());
globalMapKeyFrames.reset(new pcl::PointCloud<PointType>());
globalMapKeyFramesDS.reset(new pcl::PointCloud<PointType>());
timeLaserCloudCornerLast = 0;
timeLaserCloudSurfLast = 0;
timeLaserOdometry = 0;
timeLaserCloudOutlierLast = 0;
timeLastGloalMapPublish = 0;
timeLastProcessing = -1;
newLaserCloudCornerLast = false;
newLaserCloudSurfLast = false;
newLaserOdometry = false;
newLaserCloudOutlierLast = false;
for (int i = 0; i < 6; ++i){
transformLast[i] = 0;
transformSum[i] = 0;
transformIncre[i] = 0;
transformTobeMapped[i] = 0;
transformBefMapped[i] = 0;
transformAftMapped[i] = 0;
}
imuPointerFront = 0;
imuPointerLast = -1;
for (int i = 0; i < imuQueLength; ++i){
imuTime[i] = 0;
imuRoll[i] = 0;
imuPitch[i] = 0;
}
gtsam::Vector Vector6(6);
Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
priorNoise = noiseModel::Diagonal::Variances(Vector6);
odometryNoise = noiseModel::Diagonal::Variances(Vector6);
matA0 = cv::Mat (5, 3, CV_32F, cv::Scalar::all(0));
matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1));
matX0 = cv::Mat (3, 1, CV_32F, cv::Scalar::all(0));
matA1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));
matD1 = cv::Mat (1, 3, CV_32F, cv::Scalar::all(0));
matV1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));
isDegenerate = false;
matP = cv::Mat (6, 6, CV_32F, cv::Scalar::all(0));
laserCloudCornerFromMapDSNum = 0;
laserCloudSurfFromMapDSNum = 0;
laserCloudCornerLastDSNum = 0;
laserCloudSurfLastDSNum = 0;
laserCloudOutlierLastDSNum = 0;
laserCloudSurfTotalLastDSNum = 0;
potentialLoopFlag = false;
aLoopIsClosed = false;
latestFrameID = 0;
}