ROS-3DSLAM（四）lvi-sam源代码阅读2

最新推荐文章于 2024-08-08 07:22:13 发布

handsome_hhh

最新推荐文章于 2024-08-08 07:22:13 发布

阅读量769

点赞数 2

分类专栏： ROS源代码分析文章标签： c++ slam 计算机视觉

本文链接：https://blog.csdn.net/qq_38170211/article/details/120810130

版权

ROS源代码分析专栏收录该内容

17 篇文章 22 订阅

订阅专栏

2021SC@SDUSC

（四）lvi-sam源代码阅读2

lidar_odometry

imuPreintegration.cpp

本次分析主要是对imuHandler函数的分析。

imu的回调函数 imuHandler

根据上一次的odom和偏差，用gtsam来估计当下的odom作为imu的位姿，通过静态TF变换求得雷达的位姿，将其逐个发布。

在这里插入图片描述

如图所示，由于 IMU 的频率比雷达高得多，因此在下一帧点云即时间戳到来之前，会有大量的IMU数据读入，如上图中紫色线条所示，则可以利用这段时间间隔内的IMU测量量进行预积分操作。

用上一帧激光里程计，加上从上一帧对应时刻到当前时刻 imu 预计分量，得到当前状态，也就是 imu 里程计，再将其转到 lidar 坐标系发布。

//ConstPtr 可被理解为const pointer（常量指针），理解为shared pointer to a constant message（传递常量数据的共享指针）

/*
sensor_msgs::Imu
header中包含三个参数（三个协方差矩阵）：
方向协方差，角速度协方差，加速度协方差
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
    //imuConverter 这个函数只旋转，没有平移，和真正的雷达坐标系之间还是差了一个平移的
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

    //后续与thisImu有关的都是在差一个平移的雷达坐标系中

    // publish static tf 广播
    //map_to_odm在构造函数中初始化（全0） 时间戳 父系名字 子系名字
    // // map 和 odom 坐标系完全重合(设置的)，发布 map 坐标系，实际中并没有帮助
    tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom"));

    // 两个双端队列分别是优化前后的 imu 数据，将当前帧 imu 数据添加到队列
    imuQueOpt.push_back(thisImu);
    imuQueImu.push_back(thisImu);

    // 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分已经被重新计算
    // 如果还没有执行过 odom 优化，或者上一次优化失败导致系统重置
    // 则等待一次 odom 的优化再继续函数流程
    // 这里需要先在 odomhandler 中优化一次后再进行该函数后续的工作
    if (doneFirstOpt == false)
        return;


    //获取当前imu因子图的时间戳
    double imuTime = ROS_TIME(&thisImu);
    // lastImuT_imu初始化为-1
    // 如果首次优化，则定义初始时间间隔为 1/500 秒，否则是与上一帧作差
    double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
    //更新上一帧imu因子图的时间戳
    lastImuT_imu = imuTime;

    // integrate this single imu message
    // imu预积分器添加一帧imu数据，起始时刻是上一帧激光里程计时刻
    // https://bitbucket.org/gtborg/gtsam/src/develop/gtsam/navigation/ImuFactor.cpp
    imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
                                            gtsam::Vector3(thisImu.angular_velocity.x,    thisImu.angular_velocity.y,    thisImu.angular_velocity.z), dt);

    // predict odometry
    // 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态
    /*
    NavState : 
        Rot3 R_;
        Point3 t_;
        Velocity3 v_;
    */
    // 这个函数在wiki中找不到
    //暂时认为这个函数，根据上一个nav state，以及上一个里程计的偏差，得到了一个navigation state
    gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);

    // publish odometry
    // 发布imu里程计（转到lidar系，与激光里程计同一个系）
    //*** 是imu的里程计

    //nav_msgs::Odometry 包含位置信息和速度信息
    /*

    */
    nav_msgs::Odometry odometry;
    odometry.header.stamp = thisImu.header.stamp;
    odometry.header.frame_id = "odom";
    odometry.child_frame_id = "odom_imu";

    // transform imu pose to ldiar
    //预测值currentState获得imu位姿, 再由imu到雷达变换, 获得雷达位姿（这里应该是将imu里程计的信息，从本来所在的imu坐标系 转换到 雷达的坐标系下）
    /*
    Pose3 : 
        Rot3 R_;
        Point3 t_;
    */
    gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
    //转换 compose，继承自LieGroup类
    gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);

    //在导航的消息中添加对应的信息

    /*
    geometry_msgs/PoseWithCovariance pose //表示位置的point和quaternion，以及对应的协方差矩阵
    geometry_msgs/TwistWithCovariance twist //表示速度的linear和angular，以及对应的协方差矩阵
    */

   //第一个pose：geometry_msgs/PoseWithCovariance；第二个pose：geometry_msgs/Pose，包含一个point和一个quaternion
    odometry.pose.pose.position.x = lidarPose.translation().x();
    odometry.pose.pose.position.y = lidarPose.translation().y();
    odometry.pose.pose.position.z = lidarPose.translation().z();
    odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
    odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
    odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
    odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();

    odometry.twist.twist.linear.x = currentState.velocity().x();
    odometry.twist.twist.linear.y = currentState.velocity().y();
    odometry.twist.twist.linear.z = currentState.velocity().z();
    odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
    odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
    odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
    // information for VINS initialization
    odometry.pose.covariance[0] = double(imuPreintegrationResetId);
    odometry.pose.covariance[1] = prevBiasOdom.accelerometer().x();
    odometry.pose.covariance[2] = prevBiasOdom.accelerometer().y();
    odometry.pose.covariance[3] = prevBiasOdom.accelerometer().z();
    odometry.pose.covariance[4] = prevBiasOdom.gyroscope().x();
    odometry.pose.covariance[5] = prevBiasOdom.gyroscope().y();
    odometry.pose.covariance[6] = prevBiasOdom.gyroscope().z();
    odometry.pose.covariance[7] = imuGravity;
    //发布里程计信息
    pubImuOdometry.publish(odometry);

    // publish imu path
    // 发布 imu 轨迹，但只保存 3s
    /*
        PoseStamped[]
        带有时间戳timeStamp的pose
        Pose:
            Point 表示 position位置
            Quaternion 表示 orientation方向

    */
    //imu位姿的数组
    //静态的**** 注意生存周期
    static nav_msgs::Path imuPath;
    static double last_path_time = -1;
    //当前imu因子图的时间戳
    if (imuTime - last_path_time > 0.1)
    {
        //如果imuTime>-1 就一定会进入这个if
        last_path_time = imuTime;
        //定义一个带有时间戳的pose的消息
        geometry_msgs::PoseStamped pose_stamped;
        //把这一帧imu的 时间戳、关联的坐标系id、位置相关信息 赋值给pose_stamped
        //thisImu是在原始的Imu数据旋转到雷达坐标系后的数据
        pose_stamped.header.stamp = thisImu.header.stamp;
        pose_stamped.header.frame_id = "odom";
        pose_stamped.pose = odometry.pose.pose;
        //加入Path
        imuPath.poses.push_back(pose_stamped);
        //当imuPath数组不为空 && 且数组中最前和最后两个pose的时间戳>3秒（可以理解为超时了？太长了？）
        //true： 清除队列最开头的pose，直到间距<=3s
        while(!imuPath.poses.empty() && abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)
            imuPath.poses.erase(imuPath.poses.begin());

        //获取数组中元素个数？
        if (pubImuPath.getNumSubscribers() != 0)
        {
            //数组中存在元素的话 就发布
            //给imuPath消息添加 时间戳stamp和frame_id坐标系id
            imuPath.header.stamp = thisImu.header.stamp;
            imuPath.header.frame_id = "odom";
            //发布消息
            pubImuPath.publish(imuPath);
        }
    }

    // publish transformation
    // 发布 odom 到 base_link 的变换，其实也就是到 imu 的变换
    // 因为作者采用的模型的 base_link 就是 imu
    // 可以考虑改为发布 odom 到 imu 的 TF
    tf::Transform tCur;
    //odometry.pose.pose imu里程计的pose信息（位置 和 方向）
    tf::poseMsgToTF(odometry.pose.pose, tCur);
    //StampedTransform 
    tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, "odom", "base_link");
    tfOdom2BaseLink.sendTransform(odom_2_baselink);
}
};

总结（整体初步理解）：

从imu获取了原始数据（原始数据包括方向四元数、加速度、角速度以及他们的协方差，见：sensor_msgs/Imu.msg）
将imu进行旋转变换到雷达坐标系（没有平移），得到thisImu
将thisImu加入到两个存储imu数据的双端队列中
更新当前imu因子图的时间戳，在imu预积分器中添加当前这一帧imu数据
用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态
发布imu里程计信息
将imu位姿的信息数组更新并发布
中间穿插一些tf坐标转换的操作（imu、雷达、baselink（作者设置为imu）等）

前置知识学习

imu因子图优化

https://blog.csdn.net/ljtx200888/article/details/114164685

SLAM问题的贝叶斯网络、因子图

在上一篇博客的学习中，已经了解了关于贝叶斯网络和因子图的有关知识（https://blog.csdn.net/qq_38170211/article/details/120688614）。

在 SLAM 中,由于我们有运动方程和观测方程,它们恰好表示了状态变量之间的条件概率,所以SLAM 可以自然地表达成一个动态贝叶斯网络(Dynamic Bayes Network, DBN).下图是slam问题的贝叶斯网络表示：

在这里插入图片描述

蓝色线表示运动方程，红色线为观测方程。红框表示一次观测，蓝框表示一次运动。

节点说明：

相机位姿形成的节点：x1,x2…
输入量节点：u1,u2…
路标节点：l1,l2…
观测数据节点：z1,z2…

图上的箭头表示依赖关系，而红色和蓝色框说明变量间的条件概率关系：例如红色框表示P (z 1 |x 1 , l 1 )；蓝色框表示P (x 3 |x 2 , u 3 )；

slam问题通过贝叶斯网络求解：事实上,后端优化的目标,就是在这些既有的约束之上,通过调整贝叶斯网络中随机变量的取值,使整个后验概率达到最大(使用概率图模型中的算法进行求解):

在这里插入图片描述

通过上式，我们发现最大后验概率由许多项因子乘积而成,因此该贝叶斯网络又可以转化成一个因子图(Factor Graph)，如下图所示：在这里插入图片描述

对因子图的优化,就是调整各变量的值,使它们的因子之乘积最大化：在通常的做法中,我们把各因子的条件概率取高斯分布的形式，则根据运动方程和观测方程有：

在这里插入图片描述

运动方程和观测方程

https://blog.csdn.net/Robot_Starscream/article/details/83448257

运动方程：
$x_k = f(x_k\__1,u_k,w_k)$
观测方程：
$z_k\,_,\,_j = h(y_j, x_k, v_k\,_,\,_j)$
这两个方程描述了最基本的SLAM问题：当我们知道运动测量的读数 u ,以及传感器的读数 z 时，如何求解定位问题（估计 x）和建图问题（估计 y）? 这时，我们把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

运动：

通常，机器人会携带一个测量自身运动的传感器，比如说码盘或惯性传感器。这个传感器可以测量有关运动的读数，无论什么传感器，我们都能使用一个通用的、抽象的数学模型：
$x_k = f(x_k\__1,u_k,w_k)$

$u_k是运动传感器的参数，w_k为噪声$

观测：

假设机器人在K时刻，在xk出探测到了某个路标yj，产生了一个观测数据zk,j

，用抽象的函数h来描述这个关系：
$z_k\,_,\,_j = h(y_j, x_k, v_k\,_,\,_j)$
Vk,j为这次观测的噪声。

参考ros wiki

std/msgs

- std_msgs/Header.msg

通常用于在特定坐标系中传递时间戳数据。
# Standard metadata for higher-level stamped data types. # 高级标记数据类型的标准元数据。
    
    Compact Message Definition:
//# sequence ID: consecutively increasing ID 序列ID（连续的）
uint32 seq
// 时间戳
time stamp
//#Frame this data is associated with 这个数据所关联的坐标系frame
string frame_id

sensor/msgs

此包定义了常用传感器的信息，包括相机和扫描激光测距仪。

- sensor_msgs/Imu.msg

# 这是一条消息，用于保存来自 IMU（惯性测量单元）的数据
#
# 加速度应该以 m/s^2 为单位（不是以 g 为单位），并且旋转速度应该以弧度/秒为单位
#
# 如果测量的协方差已知，则应填写（如果您只知道每个测量的方差，例如从数据表中，只需沿对角线放置）
# 全零的协方差矩阵将被解释为“协方差未知”，并且要使用数据，必须假设协方差或从其他来源获得协方差
#
# 如果您没有对数据元素之一的估计（例如，您的 IMU 不产生方向估计），请将相关协方差矩阵的元素 0 设置为 -1
# 如果您正在解释此消息，请检查每个协方差矩阵的第一个元素中的值 -1，并忽略相关的估计。

	Compact Message Definition：

std_msgs/Header header
    
geometry_msgs/Quaternion orientation // 四元数
float64[9] orientation_covariance //四元数协方差矩阵 （貌似不包括w）
geometry_msgs/Vector3 angular_velocity //角速度（3位的向量）
float64[9] angular_velocity_covariance // 角速度协方差矩阵
geometry_msgs/Vector3 linear_acceleration // 加速度（3位的向量）
float64[9] linear_acceleration_covariance // 线加速度协方差矩阵

nav/msgs

nav_msgs定义用于与导航堆栈交互的公共消息。

- nav_msgs/Odometry.msg

# 这表示对自由空间中的位置和速度的估计。
# pose应该在header.frame_id给定的坐标系中指定。
# twist应该在child_frame_id给定的坐标系中指定

	Compact Message Definition：

std_msgs/Header header
string child_frame_id
//以下两个消息类型在 geometry_msgs中定义
geometry_msgs/PoseWithCovariance pose //表示位置的point和quaternion，以及对应的协方差矩阵
geometry_msgs/TwistWithCovariance twist //表示速度的linear和angular，以及对应的协方差矩阵

- nav_msgs/Path.msg

#An array of poses that represents a Path for a robot to follow //一系列姿势，表示机器人要遵循的路径

	Compact Message Definition
std_msgs/Header header
geometry_msgs/PoseStamped[] poses //带有时间戳得poses

geometry/msgs

为常见的几何原语(如点、向量和姿态)提供消息。这些原语旨在提供公共数据类型，并促进整个系统的互操作性。

- geometry_msgs/Vector3.msg

三维向量

	Compact Message Definition：

float64 x
float64 y
float64 z

- geometry_msgs/Quaternion.msg

四元数
# This represents an orientation in free space in quaternion form.
#以四元数的形式表示自由空间中的方向
    
	Compact Message Definition：
float64 x
float64 y
float64 z
float64 w

- geometry_msgs/Points.msg

# This contains the position of a point in free space // 点在空间中的位置

	Compact Message Definition：
float64 x
float64 y
float64 z

points：坐标点；
quaternion：表示方向；
vector：没什么实际意义，是一个三维向量

- geometry_msgs/Pose.msg

# A representation of pose in free space, composed of position and orientation.  //由位置和方向组成

	Compact Message Definition：
geometry_msgs/Point position
geometry_msgs/Quaternion orientation

- geometry_msgs/Twist.msg

# This expresses velocity in free space broken into its linear and angular parts.// 分为线性部分和角度部分。

		Compact Message Definition：
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

- geometry_msgs/PoseWithCovariance.msg

# This represents a pose in free space with uncertainty. //表示在自由空间中不确定的位置

Pose pose

# Row-major representation of the 6x6 covariance matrix //6*6的协方差矩阵
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance

	Compact Message Definition:
geometry_msgs/Pose pose
float64[36] covariance

- geometry_msgs/TwistWithConvariance.msg

# This expresses velocity in free space with uncertainty. //表示在自由空间中不确定的速度

Twist twist

# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance
    
    Cpmpact Message Definition：
geometry_msgs/Twist twist
float64[36] covariance

- geometry_msgs/PosesStamped.msg

# A Pose with reference coordinate frame and timestamp
// 带有参考坐标系和时间戳

    Cpmpact Message Definition：
std_msgs/Header header
geometry_msgs/Pose pose

gtsam库学习

一个类似ros wiki的网站，提供了gtsam库的源码，并可以根据代码或者文件名进行检索，比较方便

https://bitbucket.org/gtborg/gtsam/src/develop/

PreintegratedImuMeasurements

gtsam/gtsam/navigation/ImuFactor.h

//构造函数：
 /**
   *  Constructor, initializes the class with no measurements
   *  @param bias Current estimate of acceleration and rotation rate biases
   *  @param p    Parameters, typically fixed in a single application
   */
  PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
      PreintegrationType(p, biasHat) {
    preintMeasCov_.setZero();
  }

//添加预积分项
  /**
   * Add a single IMU measurement to the preintegration.
   * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
   * @param measuredOmega Measured angular velocity (as given by the sensor)
   * @param dt Time interval between this and the last IMU measurement
   */
  void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, const double dt) override;

imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);

在PreintegratedImuMeasurement类中没有这个函数的定义，该类继承的PreintegrationType类中也没有找到相应的函数；

在PreintegratedImuMeasurement只有一个叫Predict的函数，目前只能根据这个函数推测predict函数的作用：

void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
    Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
    PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
    const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
  // use deprecated predict
  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
      omegaCoriolis, use2ndOrderCoriolis);
  pose_j = pvb.pose;
  vel_j = pvb.velocity;
}

NavState

gtsam/gtsam/navigation/NavState.h

private:

  // TODO(frank):
  // - should we rename t_ to p_? if not, we should rename dP do dT
  Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav 四元数--方向
  Point3 t_; ///< position n_t, in nav frame 位置
  Velocity3 v_; ///< velocity n_v in nav frame 速度

Pose3

gtsam/gtsam/geometry/Pose3.h

private:

  Rot3 R_; ///< Rotation gRp, between global and pose frame
  Point3 t_; ///< Translation gTp, from global origin to pose frame origin

//Rots3 在项目中是一个四元数，代表方向
// Points3 实际为Vector3，一个三维向量，表示位置

gtsam/gtsam/geometry/Rot3.h

  private://私有变量中，当使用四元数表示方向的时候，会定义一个四元数作为私有变量

#ifdef GTSAM_USE_QUATERNIONS
    /** Internal Eigen Quaternion */
    gtsam::Quaternion quaternion_;
#else
    Matrix3 rot_;
#endif