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 odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudOutlierLast; // corner feature set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDS; // corner feature set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLast; // surf feature set from odoOptimizationpcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLastDS; // downsampled corner featuer set from odoOptimizationpcl::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 closurepcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimizationpcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualizationpcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualizationdouble 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 closuredownSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for surrounding key poses of scan-to-map optimizationdownSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for global map visualizationdownSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for global map visualizationodomAftMapped.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 odoOptimizationlaserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimizationlaserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimizationlaserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimizationlaserCloudOutlierLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimizationlaserCloudOutlierLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner feature set from odoOptimizationlaserCloudSurfTotalLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimizationlaserCloudSurfTotalLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimizationlaserCloudOri.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;}

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.rhkb.cn/news/460594.html

如若内容造成侵权/违法违规/事实不符，请联系长河编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

3.2 大数据概念、特征与价值

3.2 大数据概念、特征与价值

文章目录 大数据的概念美国高德纳咨询公司的定义麦肯锡全球研究所的定义狭义和广义的大数据 大数据的特征Volume&#xff08;体积&#xff09;Variety&#xff08;种类&#xff09;Velocity&#xff08;速度&#xff09;Value&#xff08;价值&#xff09;Veracity&#xff08;…
阅读更多...
扫雷游戏（C语言详解）

扫雷游戏（C语言详解）

扫雷游戏&#xff08;C语言详解&#xff09; 放在最前面的1、前言&#xff08;扫雷游戏的简介&#xff09;2、扫雷游戏的规则&#xff08;简易版&#xff09;3、代码实现&#xff08;3.1&#xff09;提醒一下&#xff1a;( i ) 提醒1&#xff1a;( ii ) 提醒2&#xff1a; &…
阅读更多...
WPF+MVVM案例实战（十四）- 封装一个自定义消息弹窗控件（下）

WPF+MVVM案例实战（十四）- 封装一个自定义消息弹窗控件（下）

文章目录 1、案例效果2、弹窗空间使用1.引入用户控件2、按钮命令实现 3、总结4、源代码获取 1、案例效果 2、弹窗空间使用 1.引入用户控件 打开 Wpf_Examples 项目&#xff0c;在引用中添加用户控件库&#xff0c;在 MainWindow.xaml 界面引用控件库&#xff0c;代码如下&…
阅读更多...
银河麒麟v10 xrdp安装

银河麒麟v10 xrdp安装

为了解决科技被卡脖子的问题&#xff0c;国家正在大力推进软硬件系统的信创替代&#xff0c;对于一些平时对Linux操作系统不太熟练的用户来讲提出了更高的挑战和要求。本文以银河麒麟v10 24.03为例带领大家配置kylin v10的远程桌面。 最近公司为了配置信创开发新购了几台银河麒…
阅读更多...
Python小游戏17——飞机大战

Python小游戏17——飞机大战

运行结果 首先&#xff0c;你需要安装Pygame库。如果你还没有安装它&#xff0c;可以使用以下命令来安装&#xff1a; bash pip install pygame 代码&#xff1a; python import pygame import random # 初始化Pygame pygame.init() # 屏幕大小 SCREEN_WIDTH 800 SCREEN_HEIGH…
阅读更多...
国标GB28181软件EasyGBS国标GB28181网页直播平台在邮政快递场景中的应用

国标GB28181软件EasyGBS国标GB28181网页直播平台在邮政快递场景中的应用

随着电子商务的迅猛发展&#xff0c;邮政快递行业迎来了前所未有的发展机遇&#xff0c;但同时也面临着诸多挑战。如何在保障货物安全、提高运输效率的同时&#xff0c;实现全面的监控和管理&#xff0c;成为邮政快递企业亟需解决的问题。国标GB28181网页直播平台EasyGBS作为一…
阅读更多...
MFC工控项目实例二十七添加产品参数

MFC工控项目实例二十七添加产品参数

承接专栏《MFC工控项目实例二十六创建数据库》 在型号参数界面添加三个参数试验时间、最小值、最大值。变量为double m_edit_time; double m_edit_min; double m_edit_max; 1、在SEAL_PRESSURE.h中添加代码 class CProductPara { public:union{struct{...double m_edit_min;…
阅读更多...
java项目之智能学习平台系统源码(springboot)

java项目之智能学习平台系统源码(springboot)

风定落花生&#xff0c;歌声逐流水&#xff0c;大家好我是风歌&#xff0c;混迹在java圈的辛苦码农。今天要和大家聊的是一款基于springboot的智能学习平台系统。项目源码以及部署相关请联系风歌&#xff0c;文末附上联系信息 。 &#x1f495;&#x1f495;作者&#xff1a;风…
阅读更多...
Efficient Cascaded Multiscale Adaptive Network for Image Restoration 论文阅读笔记

Efficient Cascaded Multiscale Adaptive Network for Image Restoration 论文阅读笔记

Efficient Cascaded Multiscale Adaptive Network for Image Restoration 论文阅读笔记 这是新国立和新加坡管理大学发表在ECCV2024上的一篇image restoration的文章&#xff0c;提出了一个新的网络结构ECMA&#xff0c;从实验结果上看在超分&#xff0c;去噪&#xff0c;去模糊…
阅读更多...
STEAM教育是什么？从学科融合到创造力培养的全新教育模式

STEAM教育是什么？从学科融合到创造力培养的全新教育模式

近年来&#xff0c;“STEAM教育”成为教育界的热门词汇。随着技术和科学的发展&#xff0c;传统的单一学科教育已难以满足未来社会对创新型人才的需求。STEAM教育因其跨学科的特点&#xff0c;成为培养学生综合素质和创新能力的有效方式。那么&#xff0c;什么是STEAM教育&…
阅读更多...
SystemC学习（3）— APB_SRAM的建模与测试

SystemC学习（3）— APB_SRAM的建模与测试

SystemC学习&#xff08;3&#xff09;— APB_SRAM的建模与测试 一、前言 二、APB_SRAM建模 编写APB_SRAM模型文件apb_sram.h文件如下所示&#xff1a; #ifndef __APB_SRAM_H #define __APB_SRAM_H#include "systemc.h"const int ADDR_SIZE 32; const int DATA_…
阅读更多...
Spring Boot Configuration和AutoConfiguration加载逻辑和加载顺序调整

Spring Boot Configuration和AutoConfiguration加载逻辑和加载顺序调整

在spring中&#xff0c; AutoConfiguration也是一个种Configuration&#xff0c;只是AutoConfiguration是不能使用proxy的。 而且spring对于两者的加载顺序也不是一视同仁&#xff0c;是有顺序的。spring会先加载SpringBootApplication可达的且标注了Configuration的类&#x…
阅读更多...
CodeS：构建用于文本到 SQL 的开源语言模型

CodeS：构建用于文本到 SQL 的开源语言模型

发布于&#xff1a;2024 年 10 月 29 日 #RAG #Text2 SQL #NL2 SQL 语言模型在将自然语言问题转换为 SQL 查询&#xff08;文本到 SQL &#xff09;的任务中显示出良好的性能。然而&#xff0c;大多数最先进的 &#xff08;SOTA&#xff09; 方法都依赖于强大但闭源的大型语言…
阅读更多...
社区交流系统设计与实现

社区交流系统设计与实现

社区交流系统设计与实现 1. 系统概述 社区交流系统是一个基于PHP和SQL的Web应用程序&#xff0c;旨在为用户提供一个互动交流的平台。该系统允许用户注册、发布帖子、回复帖子、查看其他用户的帖子和回复&#xff0c;以及管理个人资料&#xff0c;提高用户之间的互动和信息共享…
阅读更多...
什么是字节序、大小端、高低字节、高低地址？

什么是字节序、大小端、高低字节、高低地址？

目录 1. 什么是字节序&#xff08;Endianness&#xff09;&#xff1f; 2. 什么是大小端&#xff08;Big-Endians and Little-Endian&#xff09;&#xff1f; 3. 什么时候需要用到大小端的概念&#xff1f; 4. 如何确认系统的大小端模式&#xff1f; 5. 什么是大小端定义…
阅读更多...
为什么 C 语言数组是从 0 开始计数的？

为什么 C 语言数组是从 0 开始计数的？

C 语言等大多数编程语言的数组从 0 开始而不从 1 开始&#xff0c;有两个原因&#xff1a; 第一&#xff1a;地址计算更方便 C 语言从 0 开始的话&#xff0c;array[i] 的地址就正好是&#xff1a; (array i) 如果是从 1 开始的话&#xff0c;就是 (array i - 1) 多一次计…
阅读更多...
锁升级及线程池相关

锁升级及线程池相关

锁升级 在JVM底层实现锁的过程中&#xff0c;有三类锁&#xff1a;偏斜锁、轻量级锁、重量级锁 在Java6之前&#xff0c;synchronized的实现完全依靠重量级锁&#xff08;系统内的互斥锁&#xff09;&#xff0c;从用户态转为内核态非常消耗资源。在Java6之后&#xff0c;提供…
阅读更多...
vue3+less使用主题定制（多主题定制）可切换主题

vue3+less使用主题定制（多主题定制）可切换主题

假如要使用两套主题&#xff1a;蓝色、红色 例如&#xff1a; 首先确保自己的vue3项目有less&#xff0c;这边不多做接入解释 1、在src目录下建一个styles文件夹&#xff0c;在syles文件夹下面新建两个less文件&#xff1a;theme.less和variables.less&#xff1b; theme.le…
阅读更多...
Java面试经典 150 题.P80. 删除有序数组中的重复项 II（004）

Java面试经典 150 题.P80. 删除有序数组中的重复项 II（004）

本题来自&#xff1a;力扣-面试经典 150 题 面试经典 150 题 - 学习计划 - 力扣&#xff08;LeetCode&#xff09;全球极客挚爱的技术成长平台https://leetcode.cn/studyplan/top-interview-150/ 题解&#xff1a; class Solution {public int removeDuplicates(int[] nums)…
阅读更多...
【QNAP威联通NAS系统恢复进阶教程】如果 .conf 和 md9 无法自动组装，如何恢复 NAS？

【QNAP威联通NAS系统恢复进阶教程】如果 .conf 和 md9 无法自动组装，如何恢复 NAS？

创作立场&#xff1a;原创不易&#xff0c;拒绝搬运~ hello大家好&#xff0c;我是你们的老伙伴&#xff0c;稳重的大王~ 从本期开始&#xff0c;大王将在日常教程中&#xff0c;分享一些QNAP系统故障的排除以及解决办法&#xff0c;进阶教程需要具备一定的linux基础&#xf…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023