GTSAM Tutorial learning notes

Keywords: Algorithm slam

GTSAM Tutorial learning notes

In order to learn LIO-SAM, I quickly read Robot Perception: application of factor graph in SLAM and GTSAM Tutorial shared by big brother Dong Jing in bubble robot. The content of this blog is mainly the learning notes of GTSAM Tutorial, and make some simple analysis on the practical application of GTSAM in LIO-SAM, If you have a basic understanding of GTSAM, you can skip to the third part directly.

1. Basic principles

The following figure shows a typical SLAM scenario

The observation of feature points by the robot can be constructed as a Bayesian network as shown in the left figure above x i x_i xi is in robot state, z i z_i zi is the robot observation value, l i l_i li is the feature point. The joint distribution probability of such a Bayesian network can be expressed by the following formula: P ( X , L , Z ) = P ( x 0 ) ∏ i = 1 M P ( x i ∣ x i − 1 , u i ) ∏ k = 1 K P ( z k ∣ x i k , l j k ) P(X, L, Z)=P\left(x_{0}\right) \prod_{i=1}^{M} P\left(x_{i} \mid x_{i-1}, u_{i}\right) \prod_{k=1}^{K} P\left(z_{k} \mid x_{i_{k}}, l_{j_{k}}\right) P(X,L,Z)=P(x0) i=1 Π M P(xi ∣ xi − 1, ui) k=1 Π K P(zk ∣ xik, ljk), where,
P ( x 0 ) P\left(x_{0}\right) P(x0) is a priori state probability distribution,
P ( x i ∣ x i − 1 , u i ) P\left(x_{i} \mid x_{i-1}, u_{i}\right) P(xi ∣ xi − 1, ui) indicates a known state x i − 1 x_{i-1} xi − 1 − and control quantity u i u_{i} In the case of ui# distribution, x i \boldsymbol{x}_{i} xi , probability distribution, specifically: x i = f i ( x i − 1 , u i ) + w i ⇔ x_{i}=f_{i}\left(x_{i-1}, u_{i}\right)+w_{i} \quad \Leftrightarrow xi​=fi​(xi−1​,ui​)+wi​⇔ P ( x i ∣ x i − 1 , u i ) ∝ exp ⁡ − 1 2 ∥ f i ( x i − 1 , u i ) − x i ∥ Λ i 2 P\left(x_{i} \mid x_{i-1}, u_{i}\right) \propto \exp -\frac{1}{2}\left\|f_{i}\left(x_{i-1}, u_{i}\right)-x_{i}\right\|_{\Lambda_{i}}^{2} P(xi​∣xi−1​,ui​)∝exp−21​∥fi​(xi−1​,ui​)−xi​∥Λi​2​ P ( z k ∣ x i k , l j k ) P\left(z_{k} \mid x_{i_{k}}, l_{j_{k}}\right) P(zk ∣ xik, ljk) indicates known status x i k \boldsymbol{x}_{i_{k}} xik , and l j k l_{j_{k}} In the case of ljk distribution, z k z_{k} The probability distribution of zk , is as follows: z k = h k ( x i k , l j k ) + v k ⇔ z_{k}=h_{k}\left(x_{i_{k}}, l_{j_{k}}\right)+v_{k} \quad \Leftrightarrow zk​=hk​(xik​​,ljk​​)+vk​⇔ P ( z k ∣ x i k , l j k ) ∝ exp ⁡ − 1 2 ∥ h k ( x i k , l j k ) − z k ∥ Σ k 2 P\left(z_{k} \mid x_{i_{k}}, l_{j_{k}}\right) \propto \exp -\frac{1}{2}\left\|h_{k}\left(x_{i_{k}}, l_{j_{k}}\right)-z_{k}\right\|_{\Sigma_{k}}^{2} P(zk​∣xik​​,ljk​​)∝exp−21​∥hk​(xik​​,ljk​​)−zk​∥ Σ k ﹐ 2 ﹐ if the above distributions are assumed to be Gaussian, the following factor diagram can be constructed:
We express the above factor diagram by the following formula: P ( Θ ) ∝ ∏ i ϕ i ( θ i ) ∏ { i , j } , i < j ψ i j ( θ i , θ j ) , Θ ≜ ( X , L ) P(\Theta) \propto \prod_{i} \phi_{i}\left(\theta_{i}\right) \prod_{\{i, j\}, i<j} \psi_{i j}\left(\theta_{i}, \theta_{j}\right),\Theta \triangleq(X, L) P( Θ) ∝i∏​ ϕ i​( θ i​){i,j},i<j∏​ ψ ij​( θ i​, θ j​), Θ ≜ (X,L) where ϕ 0 ( x 0 ) ∝ P ( x 0 ) \phi_{0}\left(x_{0}\right) \propto P\left(x_{0}\right) ϕ0​(x0​)∝P(x0​) ψ ( i − 1 ) i ( x i − 1 , x i ) ∝ P ( x i ∣ x i − 1 , u i ) \psi_{(i-1) i}\left(x_{i-1}, x_{i}\right) \propto P\left(x_{i} \mid x_{i-1}, u_{i}\right) ψ(i−1)i​(xi−1​,xi​)∝P(xi​∣xi−1​,ui​) ψ i k j k ( x i k , l j k ) ∝ P ( z k ∣ x i k , l j k ) \psi_{i_{k} j_{k}}\left(x_{i_{k}}, l_{j_{k}}\right) \propto P\left(z_{k} \mid x_{i_{k}}, l_{j_{k}}\right) ψ ik ∝ jk (xik, ljk) ∝ P(zk ∣ xik, ljk) in this way, we equivalent the robot state and the surrounding map points to a factor in the factor graph. In fact, the posterior probability of the whole factor graph can be modeled as the product of the posterior probability between all factors: f ( Θ ) = ∏ i f i ( Θ i ) Θ ≜ ( X , L )  for each  f i ( Θ i ) ∝ exp ⁡ ( − 1 2 ∥ h i ( Θ i ) − z i ∥ Σ i 2 ) f(\Theta)=\prod_{i} f_{i}\left(\Theta_{i}\right) \quad \Theta \triangleq(X, L) \quad \text { for each } f_{i}\left(\Theta_{i}\right) \propto \exp \left(-\frac{1}{2}\left\|h_{i}\left(\Theta_{i}\right)-z_{i}\right\|_{\Sigma_{i}}^{2}\right) f( Θ)= i∏​fi​( Θ i​) Θ ≜(X,L)   for   each   fi​( Θ i​)∝exp(−21​∥hi​( Θ i​)−zi​∥ Σ I # 2) and our goal is to maximize the total a posteriori probability: Θ ∗ increase amount excellent turn : = arg ⁡ max ⁡ Θ f ( Θ ) \Theta ^ {* incremental Optimization:} = \ underset {\ theta} {\ Arg \ Max} f (\ theta) Θ * incremental Optimization:= Θ argmax​f( Θ) The intuitive understanding is to solve what kind of robot state and map point distribution can maximize the probability of all current observations. Because it is a Gaussian probability distribution, we usually transfer the probability distribution to the logarithmic domain: arg ⁡ min ⁡ Θ ( − log ⁡ f ( Θ ) ) = arg ⁡ min ⁡ Θ 1 2 ∑ i ∥ h i ( Θ i ) − z i ∥ Σ i 2 \underset{\Theta}{\arg \min }(-\log f(\Theta))=\underset{\Theta}{\arg \min } \frac{1}{2} \sum_{i}\left\|h_{i}\left(\Theta_{i}\right)-z_{i}\right\|_{\Sigma_{i}}^{2} Θ argmin​(−logf( Θ))=Θ argmin​21​i∑​∥hi​( Θ i​)−zi​∥ Σ I # 2 # the solution of this problem is mainly through nonlinear optimization algorithms, such as Gauss Newton method or lewnberg method. The whole solution process is as follows
Here are a few points to note:

  1. Incremental Optimization: the above description is about the solution of a fixed Batch data, while the actual SLAM problem is an incremental problem. In the method of incremental optimization based on back-end optimization, the sliding window is usually used to optimize the local map, while in GTSAM, the incremental optimization is realized through the Bayers Tree built by iSAM2, as shown in the following figure, the red area table Shows the area optimized by GTSAM. GTSAM will only take the state and map points close to the current state for optimization. Even if a loop is generated, it will only be optimized around the loop area. For details of this part, see iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree.
  2. Automatic sorting: we know that whether in Gauss Newton method or lewnberg method, an important step is to solve A X = b AX=b AX=b, there are usually two methods to solve the matrix: QR decomposition and Cholesky decomposition, in which QR decomposition is as follows: Q T A = [ R 0 ] Q T b = [ d e ] Q^{T} A=\left[\begin{array}{l} R \\ 0 \end{array}\right] \quad Q^{T} b=\left[\begin{array}{l} d \\ e \end{array}\right] QTA=[R0​]QTb=[de​] R δ = d R \delta=d R δ= dCholesky is decomposed as follows: A T A δ ∗ = A T b A^{T} A \delta^{*}=A^{T} b ATAδ∗=ATb I ≜ A T A = R T R \mathcal{I} \triangleq A^{T} A=R^{T} R I≜ATA=RTR  first  R T y = A T b  and then  R δ ∗ = y \text { first } R^{T} y=A^{T} b \text { and then } R \delta^{*}=y   first   RTy=ATb   and   then   R δ * = the efficiency of Y Cholesky decomposition is one order of magnitude higher than QR decomposition in some cases, but the stability of Cholesky decomposition is not as good as QR decomposition when the matrix is approximately singular. However, no matter which decomposition method, the decomposition efficiency depends largely on the sparsity of the matrix. In GTSAM, we make our A matrix as sparse as possible through COLAMD algorithm, so as to improve the calculation efficiency.

2. Demo code

Provided in GTSAM Tutorial Demo code It describes how to construct a factor graph with loop as shown in the figure below

First, build a factor graph container

  // Create a factor graph container
  NonlinearFactorGraph graph;

Next, construct and add a priori factor, that is, the blue factor in the figure. Note that the factor is a unary edge

  // prior measurment noise model 
  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 0.1));
  // Add a prior on the first pose, setting it to the origin
  // The prior is needed to fix/align the whole trajectory at world frame
  // A prior factor consists of a mean value and a noise model (covariance matrix)
  graph.add(PriorFactor<Pose2>(Symbol('x', 1), Pose2(0, 0, 0), priorModel));

Then construct and add odometer factors, that is, the black factors in the figure, which are binary edges

  // odometry measurement noise model (covariance matrix)
  noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));

  // Add odometry factors
  // Create odometry (Between) factors between consecutive poses
  // robot makes 90 deg right turns at x3 - x5
  graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));

There is also the loop factor, that is, the red factor on the way, which is a binary edge

  // loop closure measurement noise model
  noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));

  // Add the loop closure constraint
  graph.add(BetweenFactor<Pose2>(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));

As can be seen from the above, the process of adding factors is to construct the noise model first, then add the factor type, the node connected by the factor and the initial value to the container of the factor graph, and then initialize the noise model of each node

  // initial varible values for the optimization
  // add random noise from ground truth values
  Values initials;
  initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
  initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
  initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
  initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
  initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));

Finally, build the optimizer and perform optimization

  // Use Gauss-Newton method optimizes the initial values
  GaussNewtonParams parameters;
  // print per iteration
  // optimize!
  GaussNewtonOptimizer optimizer(graph, initials, parameters);
  Values results = optimizer.optimize();

The optimized residual value of each node can be obtained through the margins class

  // Calculate marginal covariances for all poses
  Marginals marginals(graph, results);
  // print marginal covariances
  cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
  cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
  cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
  cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
  cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;

The above is the core part of the Demo code. It can be seen that the process of building factor graph by GTSAM is very simple and clear. Next, let's see how GTSAM is applied in practical projects.

3. Partial code analysis in lio-sam

The factor diagram constructed in LIO-SAM is as follows:

The above figure looks like a factor graph, but there are actually two types of factor graphs in LIO-SAM. the first factor graph is to build a factor graph with laser odometer and IMU pre product component between two laser odometers in imuintegration.cpp file to optimize the state of the current frame (including pose, speed and offset) Here, we call it pre integration factor graph. The second factor graph is to add factor graph to key frames in mapOptimization.cpp file, add laser odometer factor, GPS factor and closed-loop factor, perform factor graph optimization, and update the position and attitude of all key frames. Here, we call it key frame factor graph. Let's see the details below (refer to some code comments) LIO-SAM source code analysis):

3.1 pre integration factor diagram

The codes related to the pre integration factor graph are mainly in the IMUPreintegration class of imuintegration.cpp, which is defined as follows:

class IMUPreintegration : public ParamServer
    std::mutex mtx;

	// Subscribe and Publish 
	ros::Subscriber subImu;
	ros::Subscriber subOdometry;
	ros::Publisher pubImuOdometry;
	bool systemInitialized = false;
	// Noise covariance
	gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
	gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
	gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
	gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
	gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
	gtsam::Vector noiseModelBetweenBias;
	// imu pre integrator
	gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
	gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
	// imu data queue
	std::deque<sensor_msgs::Imu> imuQueOpt;
	std::deque<sensor_msgs::Imu> imuQueImu;
	// State variables in imu factor graph optimization
	gtsam::Pose3 prevPose_;
	gtsam::Vector3 prevVel_;
	gtsam::NavState prevState_;
	gtsam::imuBias::ConstantBias prevBias_;
	// imu status
	gtsam::NavState prevStateOdom;
	gtsam::imuBias::ConstantBias prevBiasOdom;
	bool doneFirstOpt = false;
	double lastImuT_imu = -1;
	double lastImuT_opt = -1;
	// ISAM2 optimizer
	gtsam::ISAM2 optimizer;
	gtsam::NonlinearFactorGraph graphFactors;
	gtsam::Values graphValues;
	const double delta_t = 0;
	// Used to record the number of keyframes
	int key = 1;
	// IMU lidar pose transformation
	gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
	gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));

	// Constructor

	// Reset iSAM2 optimizer
    void resetOptimization();

	// reset parameters 
    void resetParams();

	// Subscription laser odometer
    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg);

	// Determination of IMU factor graph optimization results
    bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur);

	// Subscribe to raw IMU data
    void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw);

The member variables of the class include each state to be optimized and the noise covariance of each a priori factor. These parameters are initialized in the constructor. Let's see the contents of the constructor below:

    // Subscribe to the original imu data, apply the imu prediction component between two frames with the optimization result of the following factor diagram, and predict the imu odometer at each time (imu frequency)
    subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
    // Subscribe to the laser odometer from mapOptimization, use the estimated imu component between two frames to build a factor map, and optimize the pose of the current frame (this pose is only used to update the imu odometer at each time and the next factor map optimization)
    subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
    // Issue imu odometer
    pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);

    // Noise covariance of imu pre integration
    boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
    p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
    p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
    p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
    gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias

    // Noise a priori
    priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
    priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
    priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
    // If the laser odometer degrades in the process of scan to map optimization, a large covariance is selected
    correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
    correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
    noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
    // imu pre integrator, used to predict imu odometer at each time (imu frequency) (switch to lidar system, the same system as laser odometer)
    imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
    // imu pre integrator for factor graph optimization
    imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        

Next, the core part of constructing the factor graph is in the subscription laser odometer function, as follows:

 * Subscribe to laser odometer from mapOptimization
 * 1,Reset the ISAM2 optimizer every 100 frames, add odometer, speed and bias a priori factors, and perform optimization
 * 2,Calculate the imu pre product component between the previous frame laser odometer and the current frame laser odometer, apply the pre product component with the previous frame state to obtain the initial state estimation of the current frame, add the current frame pose from mapOptimization, optimize the factor graph and update the current frame state
 * 3,After optimization, re propagation is performed; The imu offset is optimized and updated. The imu pre integration after the current laser odometer time is recalculated with the latest offset. This pre integration is used to calculate the pose at each time
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    std::lock_guard<std::mutex> lock(mtx);
    // Time stamp of laser odometer in current frame
    double currentCorrectionTime = ROS_TIME(odomMsg);

    // Ensure that there is imu data in the imu optimization queue for pre integration
    if (imuQueOpt.empty())

    // The laser pose of the current frame comes from the pose after scan to map matching and factor map optimization
    float p_x = odomMsg->pose.pose.position.x;
    float p_y = odomMsg->pose.pose.position.y;
    float p_z = odomMsg->pose.pose.position.z;
    float r_x = odomMsg->pose.pose.orientation.x;
    float r_y = odomMsg->pose.pose.orientation.y;
    float r_z = odomMsg->pose.pose.orientation.z;
    float r_w = odomMsg->pose.pose.orientation.w;
    bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
    gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

    // 0. System initialization, first frame
    if (systemInitialized == false)
        // Reset ISAM2 optimizer

        // Delete the imu data before the laser odometer time of the current frame from the imu optimization queue
        while (!imuQueOpt.empty())
            if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                lastImuT_opt = ROS_TIME(&imuQueOpt.front());
        // Add odometer pose prior factor
        prevPose_ = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
        // Add speed a priori factor
        prevVel_ = gtsam::Vector3(0, 0, 0);
        gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
        // Add imu bias a priori factor
        prevBias_ = gtsam::imuBias::ConstantBias();
        gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
        // Initial value of variable node
        graphValues.insert(X(0), prevPose_);
        graphValues.insert(V(0), prevVel_);
        graphValues.insert(B(0), prevBias_);
        // Optimize once
        optimizer.update(graphFactors, graphValues);
        // Reset optimized offset
        key = 1;
        systemInitialized = true;

    // Reset the ISAM2 optimizer every 100 frames to ensure the optimization efficiency
    if (key == 100)
        // Pose, velocity and bias noise model of the previous frame
        gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
        gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
        gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
        // Reset ISAM2 optimizer
        // Add pose a priori factor and initialize with the value of the previous frame
        gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
        // Add a speed a priori factor and initialize with the value of the previous frame
        gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
        // Add an offset a priori factor and initialize with the value of the previous frame
        gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
        // The variable node is assigned an initial value and initialized with the value of the previous frame
        graphValues.insert(X(0), prevPose_);
        graphValues.insert(V(0), prevVel_);
        graphValues.insert(B(0), prevBias_);
        // Optimize once
        optimizer.update(graphFactors, graphValues);

        key = 1;

    // 1. Calculate the imu pre product component between the previous frame and the current frame, apply the pre product component with the previous frame state to obtain the initial state estimation of the current frame, add the current frame pose from mapOptimization, optimize the factor graph and update the current frame state
    while (!imuQueOpt.empty())
        // Extract the imu data between the previous frame and the current frame and calculate the pre integration
        sensor_msgs::Imu *thisImu = &imuQueOpt.front();
        double imuTime = ROS_TIME(thisImu);
        if (imuTime < currentCorrectionTime - delta_t)
            // Time interval between two frames of imu data
            double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
            // imu pre integration data input: acceleration, angular velocity, dt
                    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);
            lastImuT_opt = imuTime;
            // Delete the processed imu data from the queue
    // Add imu pre integration factor
    const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
    // Parameters: pose of previous frame, velocity of previous frame, pose of current frame, velocity of current frame, offset of previous frame, pre product component
    gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
    // Add imu offset factor, previous frame offset, current frame offset, observation value and noise covariance; deltaTij() is the time of the integration section
    graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                        gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
    // Add pose factor
    gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
    gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
    // Using the state and offset of the previous frame, the imu prediction component is applied to obtain the state of the current frame
    gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
    // Initial value of variable node
    graphValues.insert(X(key), propState_.pose());
    graphValues.insert(V(key), propState_.v());
    graphValues.insert(B(key), prevBias_);
    // optimization
    optimizer.update(graphFactors, graphValues);
    // Optimization results
    gtsam::Values result = optimizer.calculateEstimate();
    // Update the pose and speed of the current frame
    prevPose_  =<gtsam::Pose3>(X(key));
    prevVel_   =<gtsam::Vector3>(V(key));
    // Update current frame status
    prevState_ = gtsam::NavState(prevPose_, prevVel_);
    // Update current frame imu offset
    prevBias_  =<gtsam::imuBias::ConstantBias>(B(key));
    // Reset the pre integrator and set a new offset, so that when the laser odometer comes in the next frame, the pre integrator component is the increment between two frames

    // If the speed or offset of imu factor graph optimization result is too large, it is considered as failure
    if (failureDetection(prevVel_, prevBias_))
        // reset parameters 

    // 2. After optimization, perform re propagation; The imu offset is optimized and updated. The imu pre integration after the current laser odometer time is recalculated with the latest offset. This pre integration is used to calculate the pose at each time
    prevStateOdom = prevState_;
    prevBiasOdom  = prevBias_;
    // Delete the imu data before the current laser odometer time from the imu queue
    double lastImuQT = -1;
    while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        lastImuQT = ROS_TIME(&imuQueImu.front());
    // Pre integration is calculated for the remaining imu data
    if (!imuQueImu.empty())
        // Reset the pre integrator and the latest offset
        // Calculate pre integration
        for (int i = 0; i < (int)imuQueImu.size(); ++i)
            sensor_msgs::Imu *thisImu = &imuQueImu[i];
            double imuTime = ROS_TIME(thisImu);
            double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

            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);
            lastImuQT = imuTime;

    doneFirstOpt = true;

One of the more special points is the use of the gtsam::PreintegratedImuMeasurements class. Finding a class is equivalent to encapsulating a pile of preintegrations and their Jacobian derivation formulas. It's not too cool. We can also see from the code that the slightly more complex part in the process of building factor graph is the given process of Noise Model of factor.

3.2 key frame factor diagram

The codes related to the key frame factor graph are mainly in the savekeyframeandfactor function of mapOptimization.cpp. In mapOptimization.cpp, in addition to the key frame factor graph, it also includes the matching algorithm of scan to map, key frame determination, etc. we will not introduce these codes too much. The savekeyframeandfactor function is as follows:

 * Set the current frame as a key frame and perform factor graph optimization
 * 1,Calculate the pose transformation between the current frame and the previous frame. If the change is too small, it will not be set as a key frame, otherwise it will be set as a key frame
 * 2,Add laser odometer factor, GPS factor and closed-loop factor
 * 3,Execution factor graph optimization
 * 4,The optimized pose and pose covariance of the current frame are obtained
 * 5,Add cloudKeyPoses3D and cloudKeyPoses6D, update transformTobeMapped, and add the corner and plane point collection of the current key frame
void saveKeyFramesAndFactor()
    // Calculate the pose transformation between the current frame and the previous frame. If the change is too small, it will not be set as a key frame, otherwise it will be set as a key frame
    if (saveFrame() == false)

    // Laser odometer factor

    // GPS factor

    // Closed loop factor

    // cout << "****************************************************" << endl;
    // gtSAMgraph.print("GTSAM Graph:\n");
    // Execution optimization
    isam->update(gtSAMgraph, initialEstimate);

    if (aLoopIsClosed == true)

    // After update, clear the saved factor graph. Note: the historical data will not be cleared, but the ISAM will be saved

    PointType thisPose3D;
    PointTypePose thisPose6D;
    Pose3 latestEstimate;

    // Optimization results
    isamCurrentEstimate = isam->calculateEstimate();
    // Pose result of current frame
    latestEstimate =<Pose3>(isamCurrentEstimate.size()-1);
    // cout << "****************************************************" << endl;
    // isamCurrentEstimate.print("Current estimate: ");

    // cloudKeyPoses3D adds the pose of the current frame
    thisPose3D.x = latestEstimate.translation().x();
    thisPose3D.y = latestEstimate.translation().y();
    thisPose3D.z = latestEstimate.translation().z();
    // Indexes
    thisPose3D.intensity = cloudKeyPoses3D->size(); 

    // Cloudkeypose6d adds the pose of the current frame
    thisPose6D.x = thisPose3D.x;
    thisPose6D.y = thisPose3D.y;
    thisPose6D.z = thisPose3D.z;
    thisPose6D.intensity = thisPose3D.intensity ; 
    thisPose6D.roll  = latestEstimate.rotation().roll();
    thisPose6D.pitch = latestEstimate.rotation().pitch();
    thisPose6D.yaw   = latestEstimate.rotation().yaw();
    thisPose6D.time = timeLaserInfoCur;

    // cout << "****************************************************" << endl;
    // cout << "Pose covariance:" << endl;
    // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
    // Pose covariance
    poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);

    // transformTobeMapped updates the pose of the current frame
    transformTobeMapped[0] = latestEstimate.rotation().roll();
    transformTobeMapped[1] = latestEstimate.rotation().pitch();
    transformTobeMapped[2] = latestEstimate.rotation().yaw();
    transformTobeMapped[3] = latestEstimate.translation().x();
    transformTobeMapped[4] = latestEstimate.translation().y();
    transformTobeMapped[5] = latestEstimate.translation().z();

    // Current frame laser corner, plane point, downsampling set
    pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
    pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
    pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);

    // Save feature point downsampling set

    // Update odometer track

The optimization results of all state variables in history are saved in isamCurrentEstimate, while the results of the latest frame are saved in latestEstimate, where the function of adding odometer factor is

void addOdomFactor()
    if (cloudKeyPoses3D->points.empty())
        noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
        gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
        initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
        gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
        gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
        gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
        initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);

Adding GPS factor function is relatively complex, so it is necessary to judge whether to add and interpolate:

void addGPSFactor()
    if (gpsQueue.empty())

    // wait for system initialized and settles down
    if (cloudKeyPoses3D->points.empty())
        if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)

    // pose covariance small, no need to correct
    if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)

    // last gps position
    static PointType lastGPSPoint;

    while (!gpsQueue.empty())
        if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
            // message too old
        else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
            // message too new
            nav_msgs::Odometry thisGPS = gpsQueue.front();

            // GPS too noisy, skip
            float noise_x = thisGPS.pose.covariance[0];
            float noise_y = thisGPS.pose.covariance[7];
            float noise_z = thisGPS.pose.covariance[14];
            if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)

            float gps_x = thisGPS.pose.pose.position.x;
            float gps_y = thisGPS.pose.pose.position.y;
            float gps_z = thisGPS.pose.pose.position.z;
            if (!useGpsElevation)
                gps_z = transformTobeMapped[5];
                noise_z = 0.01;

            // GPS not properly initialized (0,0,0)
            if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)

            // Add GPS every a few meters
            PointType curGPSPoint;
            curGPSPoint.x = gps_x;
            curGPSPoint.y = gps_y;
            curGPSPoint.z = gps_z;
            if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
                lastGPSPoint = curGPSPoint;

            gtsam::Vector Vector3(3);
            Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
            noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
            gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);

            aLoopIsClosed = true;

Add the loopback factor function as follows:

void addLoopFactor()
    if (loopIndexQueue.empty())

    for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
        int indexFrom = loopIndexQueue[i].first;
        int indexTo = loopIndexQueue[i].second;
        gtsam::Pose3 poseBetween = loopPoseQueue[i];
        gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
        gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));

    aLoopIsClosed = true;

Then, the above briefly introduces the application of GTSAM in leo-sam project. The introduction is still rough. I will add it slowly later. I have just contacted GTSAM. If you have any questions, welcome to communicate!

Posted by helraizer on Thu, 07 Oct 2021 09:39:05 -0700