VIO第5讲：后端优化实践

兔子不吃草～2024-02-26 10:51

VIO第5讲后端优化实践：逐行手写求解器

文章目录

  • VIO第5讲后端优化实践：逐行手写求解器
  • [1 非线性最小二乘求解流程](#1 非线性最小二乘求解流程)
    • [1.1 H矩阵不满秩的解决办法](#1.1 H矩阵不满秩的解决办法)
    • [1.2 H矩阵的构建](#1.2 H矩阵的构建)
      • [1.2.1 确定维度](#1.2.1 确定维度)
      • [1.2.2 构建海塞矩阵](#1.2.2 构建海塞矩阵)
    • [1.3 初始化μ---LM算法](#1.3 初始化μ—LM算法)
    • [1.4 求解线性方程](#1.4 求解线性方程)
      • [1.4.1 非SLAM问题---求逆](#1.4.1 非SLAM问题—求逆)
      • [1.4.2 SLAM问题---舒尔补](#1.4.2 SLAM问题—舒尔补)
        • [① 根据pose和landmark分块](#① 根据pose和landmark分块)
        • [② 舒尔补](#② 舒尔补)
        • [③ 求解线性方程组](#③ 求解线性方程组)
  • [2 marginalize测试](#2 marginalize测试)
    • [2.1 移动H矩阵中边缘化的量](#2.1 移动H矩阵中边缘化的量)
    • [2.2 舒尔补-边缘化](#2.2 舒尔补-边缘化)
  • [3 添加先验prior约束](#3 添加先验prior约束)
  • [4 其余部分程序](#4 其余部分程序)

1 非线性最小二乘求解流程

1.1 H矩阵不满秩的解决办法

解决办法1：使用LM算法，加阻尼因子使得系统满秩！可求解，但是求得的结

果可能会往零空间变化。结果之间的相对关系是不会发生变化的，但是整体可能与实际结果都差一个相对变换T，所以需要我们变换到原始结果上。
解决办法2：添加先验约束，增加系统的可观性。g2o、ceres中对系统的第一个pose的信息矩阵加上单位阵 H [ 11 ] + = I . \mathbf{H}_{[11]}+=\mathbf{I}. H[11]+=I.
解决办法3：固定一个相机pose和一个特征点，或者固定两个相机pose，限定优化值不乱飘。

cpp 复制代码 
    // 所有 Pose
    vector<shared_ptr<VertexPose> > vertexCams_vec;
    for (size_t i = 0; i < cameras.size(); ++i) {
        shared_ptr<VertexPose> vertexCam(new VertexPose());
        Eigen::VectorXd pose(7);
        pose << cameras[i].twc, cameras[i].qwc.x(), cameras[i].qwc.y(), cameras[i].qwc.z(), cameras[i].qwc.w(); //平移和四元数
        vertexCam->SetParameters(pose);

            //    if(i < 2)		// 这里就是解决办法3
            //        vertexCam->SetFixed();

        problem.AddVertex(vertexCam);
        vertexCams_vec.push_back(vertexCam);
    }

1.2 H矩阵的构建

海塞矩阵的构建主要有一下流程

1.2.1 确定维度

1 确定维度-----优化变量的维度n = 6p+3l，残差维度m，则单个残差雅可比J（1*n），推断出单个H（n*n）。然后把m个残差对应的H矩阵叠加拼接！

ordering_generic_ += vertex.second->LocalDimension(); 是所有优化变量的总维度，也是H矩阵的维度！

MatXX H(MatXX::Zero(ordering_generic_, ordering_generic_))

其中LocalDimension()是优化变量要优化的维度，比如位姿，如果用四元数表示，输入参数维度是7，但实际优化维度是6！

cpp 复制代码 
// 统计优化变量的维数，为构建 H 矩阵做准备
void Problem::SetOrdering() {

    // 每次重新计数
    ordering_poses_ = 0;
    ordering_generic_ = 0;
    ordering_landmarks_ = 0;
    int debug = 0;

    // Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
    for (auto vertex: verticies_) {
        ordering_generic_ += vertex.second->LocalDimension();  // 所有的优化变量总维数

        if (IsPoseVertex(vertex.second)) {
            debug += vertex.second->LocalDimension();
        }

        if (problemType_ == ProblemType::SLAM_PROBLEM)    // 如果是 slam 问题，还要分别统计 pose 和 landmark 的维数，后面会对他们进行排序
        {
            AddOrderingSLAM(vertex.second);
        }

        if (IsPoseVertex(vertex.second)) {
            std::cout << vertex.second->Id() << " order: " << vertex.second->OrderingId() << std::endl;
        }
    }
    
        if (problemType_ == ProblemType::SLAM_PROBLEM) {
        // 这里要把 landmark 的 ordering 加上 pose 的数量，就保持了 landmark 在后,而 pose 在前
        ulong all_pose_dimension = ordering_poses_;
        for (auto landmarkVertex : idx_landmark_vertices_) {
            landmarkVertex.second->SetOrderingId(
                landmarkVertex.second->OrderingId() + all_pose_dimension
            );
        }
    }
}

ordering_poses_是海塞矩阵中每个优化姿态的实际索引，因为每一个姿态占6维，所以要每次更新其在H中的左上角索引！AddOrderingSLAM会对其每一个位姿顶点进行更新，并按照这个顺序对位姿顶点进行排序idx_pose_vertices_！

cpp 复制代码 
void Problem::AddOrderingSLAM(std::shared_ptr<myslam::backend::Vertex> v) {
    if (IsPoseVertex(v)) {  // Pose顶点
        v->SetOrderingId(ordering_poses_);
        idx_pose_vertices_.insert(pair<ulong, std::shared_ptr<Vertex>>(v->Id(), v));
        ordering_poses_ += v->LocalDimension();     // 顶点实际优化的维度，比如姿态就是6+6+6+6+...
    } else if (IsLandmarkVertex(v)) {
        v->SetOrderingId(ordering_landmarks_);
        ordering_landmarks_ += v->LocalDimension();
        idx_landmark_vertices_.insert(pair<ulong, std::shared_ptr<Vertex>>(v->Id(), v));
    }
}

1.2.2 构建海塞矩阵

这里详细的分析下海塞矩阵的构建，以下面这个图为例

一个误差边edge，图中显示连接两个顶点，即二元边。

以误差 r 13 r_{13} r13为例，std::vector<MatXX> jacobians = edge.second->Jacobians()取出雅可比矩阵，是一个vcector向量，比如这里size = 2 = 顶点数，一个是对顶点1，一个是对顶点3。assert也可验证。

为了方便写代码，这里并不是直接用 J 2 ⊤ Σ 2 − 1 J \mathbf{J}{2}^{\top}\mathbf{\Sigma}{2}^{-1}\mathbf{J} J2⊤Σ2−1J这种直接相乘的，而是进行了拆分，如下图

实际代码中两个for循环计算的hessian是6*6或3*3或6*3或3*6小雅可比矩阵，通过找到其在大雅可比矩阵中的索引，进行填充。

H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;

index_i,index_j就是两个顶点对应的索引6p+3l，dim_i, dim_j就是有多种组合，看是位姿与位姿约束，或位姿与路标点约束等。

H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();这里有个小技巧，因为H是对称的，我们计算(i,j)时，可以直接填写(j,i)。

最后要注意的一点就是残差b的计算，就像纸上面写的，只在i这一循环计算

cpp 复制代码 
void Problem::MakeHessian() {
    TicToc t_h;
    // 直接构造大的 H 矩阵
    ulong size = ordering_generic_;             // 6n
    MatXX H(MatXX::Zero(size, size));           // 6n*6n
    VecX b(VecX::Zero(size));                   // 6n*1

    for (auto &edge: edges_) {

        edge.second->ComputeResidual();
        edge.second->ComputeJacobians();

        auto jacobians = edge.second->Jacobians();      // std::vector<MatXX>
        auto verticies = edge.second->Verticies();

        // 因为对一个优化变量的雅可比就是一个矩阵了，所以对所有优化变量的雅可比用vector来统计
        assert(jacobians.size() == verticies.size());   // std::vector<MatXX>大小应该是与顶点数相同的

        for (size_t i = 0; i < verticies.size(); ++i) {
            auto v_i = verticies[i];
            if (v_i->IsFixed()) continue;    // Hessian 里不需要添加它的信息，也就是它的雅克比为 0

            auto jacobian_i = jacobians[i];
            ulong index_i = v_i->OrderingId();	// 顶点在H实际索引
            ulong dim_i = v_i->LocalDimension();// 优化参数维度6/3

            MatXX JtW = jacobian_i.transpose() * edge.second->Information();
            for (size_t j = i; j < verticies.size(); ++j) {
                auto v_j = verticies[j];

                if (v_j->IsFixed()) continue;

                auto jacobian_j = jacobians[j];
                ulong index_j = v_j->OrderingId();
                ulong dim_j = v_j->LocalDimension();

                assert(v_j->OrderingId() != -1);
                MatXX hessian = JtW * jacobian_j;
                // 所有的信息矩阵叠加起来
                // TODO:: home work. 完成 H index 的填写.
                // H.block(?,?, ?, ?).noalias() += hessian;
                H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;

                // 对称矩阵，我们从j=i开始遍历，实际上只能遍历矩阵的一半，同时利用对称矩阵的性质，构建对称部分！
                if (j != i) {   // 对称的下三角
                    
		            // TODO:: home work. 完成 H index 的填写.
                    // H.block(?,?, ?, ?).noalias() += hessian.transpose();
                    H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();
                }
            }
            b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
        }

    }
    Hessian_ = H;
    b_ = b;
    t_hessian_cost_ += t_h.toc();

    if (err_prior_.rows() > 0) {
        b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_);   // update the error_prior
    }
    Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_;
    b_.head(ordering_poses_) += b_prior_;

    delta_x_ = VecX::Zero(size);  // initial delta_x = 0_n;

}

1.3 初始化μ---LM算法

μ 0 = τ ⋅ max ⁡ { ( J ⊤ J ) i i } \mu_0=\tau\cdot\max\left\{\left(\mathbf{J}^\top\mathbf{J}\right)_{ii}\right\} μ0=τ⋅max{(J⊤J)ii}

cpp 复制代码 
void Problem::ComputeLambdaInitLM() {
    
	...
    
    for (ulong i = 0; i < size; ++i) {
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
    }
    double tau = 1e-5;
    currentLambda_ = tau * maxDiagonal;
}

1.4 求解线性方程

到次为止，H有了，残差有了，LM参数有了，那么我们就能够直接求解这个方程

SolveLinearSystem函数分析

1.4.1 非SLAM问题---求逆

H矩阵是稠密矩阵，直接求逆

cpp 复制代码 
    if (problemType_ == ProblemType::GENERIC_PROBLEM) {

        // 非 SLAM 问题直接求解
        // PCG solver
        MatXX H = Hessian_;
        for (ulong i = 0; i < Hessian_.cols(); ++i) {
            H(i, i) += currentLambda_;
        }
//        delta_x_ = PCGSolver(H, b_, H.rows() * 2);
        delta_x_ = Hessian_.inverse() * b_;

    }

1.4.2 SLAM问题---舒尔补

H矩阵是稀疏矩阵，利用其稀疏特性，用schur消元进行边缘化marginalize操作，加速求解

[ H p p H p m H m p H m m ] [ Δ x p ∗ Δ x m ∗ ] = [ b p p b m m ] \begin{bmatrix}H_{pp}&H_{pm}\\H_{mp}&H_{mm}\end{bmatrix}\begin{bmatrix}\Delta x_{p}^{*}\\\Delta x_{m}^{*}\end{bmatrix}=\begin{bmatrix}b_{pp}\\b_{mm}\end{bmatrix} [HppHmpHpmHmm][Δxp∗Δxm∗]=[bppbmm]

① 根据pose和landmark分块

矩阵维度 H ( p + m ) ∗ ( p + m ) ∗ Δ x ( p + m ) ∗ 1 = b ( p + m ) ∗ 1 H_{(p+m)*(p+m)}*Δx_{(p+m)*1} = b_{(p+m)*1} H(p+m)∗(p+m)∗Δx(p+m)∗1=b(p+m)∗1，填充相应位置

ordering_poses_是所有的位姿顶点维度 = 6*位姿顶点个数，即这里的p

ordering_landmarks_是所有的路标点顶点维度 = 3 * 路标点顶点个数，即m

cpp 复制代码 
// SLAM 问题采用舒尔补的计算方式
// step1: schur marginalization --> Hpp, bpp
int reserve_size = ordering_poses_;
int marg_size = ordering_landmarks_;

// TODO:: home work. 完成矩阵块取值，Hmm，Hpm，Hmp，bpp，bmm
// Hmm(p,p,m,m)
MatXX Hmm = Hessian_.block(reserve_size,reserve_size, marg_size, marg_size);
// Hpm(0,p,p,m)
MatXX Hpm = Hessian_.block(0,reserve_size, reserve_size, marg_size);
// Hmp(p,0,m,p)
MatXX Hmp = Hessian_.block(reserve_size,0, marg_size, reserve_size);

// 注意这不是行列索引，对于列向量，segment这里指的是起始值
VecX bpp = b_.segment(0,reserve_size);
VecX bmm = b_.segment(reserve_size,marg_size);
② 舒尔补

矩阵 H m m H_{mm} Hmm和 H p p H_{pp} Hpp都是对角矩阵，即可逆。我们这里选择计算矩阵的舒尔补，即 ( H p p − H p m H m m − 1 H m p ) (H_{pp}-H_{pm}H_{mm}^{-1}H_{mp}) (Hpp−HpmHmm−1Hmp)

1 计算右下角关于路标点 的矩阵Hmm之逆矩阵 H m m − 1 H_{mm}^{-1} Hmm−1

cpp 复制代码 
// Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
for (auto landmarkVertex : idx_landmark_vertices_) {
    // OrderingId()返回的之在H中的索引，要减去前面关于位姿的维度
    int idx = landmarkVertex.second->OrderingId() - reserve_size;
    
    int size = landmarkVertex.second->LocalDimension();     // 3*3
    
    Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}

2 计算 ( H p p − H p m H m m − 1 H m p ) (H_{pp}-H_{pm}H_{mm}^{-1}H_{mp}) (Hpp−HpmHmm−1Hmp)

左乘矩阵，将H矩阵变为下三角矩阵
[ I − H p m H m m − 1 0 I ] [ H p p H p m H m p H m m ] [ Δ x p ∗ Δ x c ∗ ] = [ I − H p m H m m − 1 0 I ] [ b p p b m m ] . \begin{bmatrix}I&-H_{pm}H_{mm}^{-1}\\0&I\end{bmatrix}\begin{bmatrix}H_{pp}&H_{pm}\\H_{mp}&H_{mm}\end{bmatrix}\begin{bmatrix}\Delta x_\mathrm{p}^{*}\\\Delta x_c^{*}\end{bmatrix}=\begin{bmatrix}I&-H_{pm}H_{mm}^{-1}\\0&I\end{bmatrix}\begin{bmatrix}b_{pp}\\b_{mm}\end{bmatrix}. [I0−HpmHmm−1I][HppHmpHpmHmm][Δxp∗Δxc∗]=[I0−HpmHmm−1I][bppbmm].

[ ( H p p − H p m H m m − 1 H m p ) 0 H m p H m m ] [ Δ x p ∗ Δ x c ∗ ] = [ b p p − H p m H m m − 1 b m m b m m ] \begin{bmatrix}\left(H_{pp}-H_{pm}H_{mm}^{-1}H_{mp}\right)&\mathbf{0}\\H_{mp}&H_{mm}\end{bmatrix}\begin{bmatrix}\Delta x_\mathrm{p}^{*}\\\Delta x_c^{*}\end{bmatrix}=\begin{bmatrix}b_{pp}-H_{pm}H_{mm}^{-1}b_{mm}\\b_{mm}\end{bmatrix} [(Hpp−HpmHmm−1Hmp)Hmp0Hmm][Δxp∗Δxc∗]=[bpp−HpmHmm−1bmmbmm]

cpp 复制代码 
// Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
for (auto landmarkVertex : idx_landmark_vertices_) {
    int idx = landmarkVertex.second->OrderingId() - reserve_size;
    int size = landmarkVertex.second->LocalDimension();     // 3*3
    Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}

( H p p − H p m H m m − 1 H m p ) Δ x p ∗ = b p p − H p m H m m − 1 b m m \left(H_{pp}-H_{pm}H_{mm}^{-1}H_{mp}\right)\Delta x_{p}^{*}=b_{pp}-H_{pm}H_{mm}^{-1}b_{mm} (Hpp−HpmHmm−1Hmp)Δxp∗=bpp−HpmHmm−1bmm

cpp 复制代码 
// TODO:: home work. 完成舒尔补 Hpp, bpp 代码
MatXX tempH = Hpm * Hmm_inv;
H_pp_schur_ = Hessian_.block(0,0,reserve_size,reserve_size) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bmm;
③ 求解线性方程组

求 Δ x p ∗ \Delta x_{p}^{*} Δxp∗

cpp 复制代码 
 VecX delta_x_pp(VecX::Zero(reserve_size));
// PCG Solver
for (ulong i = 0; i < ordering_poses_; ++i) {
    H_pp_schur_(i, i) += currentLambda_;
}

int n = H_pp_schur_.rows() * 2;                       // 迭代次数
delta_x_pp = PCGSolver(H_pp_schur_, b_pp_schur_, n);  // 哈哈，小规模问题，搞 pcg 花里胡哨
delta_x_.head(reserve_size) = delta_x_pp;

​ 已知 Δ x p ∗ \Delta x_{p}^{*} Δxp∗，求 Δ x m ∗ \Delta x_{m}^{*} Δxm∗

H m m Δ x m ∗ = b m − H m p Δ x p ∗ H_{mm}\Delta x_m^*=b_m-H_{mp}\Delta x_p^* HmmΔxm∗=bm−HmpΔxp∗

cpp 复制代码 
// TODO:: home work. step3: solve landmark
VecX delta_x_ll(marg_size);
// delta_x_ll = ???;
delta_x_ll = Hmm_inv * (bmm - Hmp * delta_x_pp);
delta_x_.tail(marg_size) = delta_x_ll;

2 marginalize测试

在上面解决位姿优化问题中并没有真正使用marg，而是在最后给了一个具体的信息矩阵H，把对齐操作的结果展示了出来。

2.1 移动H矩阵中边缘化的量

cpp 复制代码 
int idx = 1;            // marg 中间那个变量
int dim = 1;            // marg 变量的维度
int reserve_size = 3;   // 总共变量的维度
double delta1 = 0.1 * 0.1;
double delta2 = 0.2 * 0.2;
double delta3 = 0.3 * 0.3;

int cols = 3;
MatXX H_marg(MatXX::Zero(cols, cols));
H_marg << 1./delta1, -1./delta1, 0,
-1./delta1, 1./delta1 + 1./delta2 + 1./delta3, -1./delta3,
0.,  -1./delta3, 1/delta3;

// TODO:: home work. 将变量移动到右下角

/// 准备工作： move the marg pose to the Hmm bottown right

1 将 row i 移动矩阵最下面

把第idx行移动到矩阵的最后一行，首先取出第idx行，行维dim，列就是矩阵维度reserve_size。

取出矩阵idx行下面的全部，对于开始索引即idx + dim, 0。要取得矩阵块大小应该是reserve_size- idx - dim，比如矩阵维度是10，idx是1，dim是1，那么取得大小就是（10 - 1 - 1）*10 。

cpp 复制代码 
Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
H_marg.block(idx,0,reserve_size - idx - dim, reserve_size) = temp_botRows;
H_marg.block(reserve_size - dim, 0, dim, reserve_size) = temp_rows;

2 将 col i 移动矩阵最右边

cpp 复制代码 
Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;

2.2 舒尔补-边缘化

本质上就是上面推导舒尔补过程，就是把这个边缘化后的先验矩阵H_prior给出来了，实质上也没有什么特别的。

cpp 复制代码 
    /// 开始 marg ： schur
    double eps = 1e-8;
    int m2 = dim;
    int n2 = reserve_size - dim;   // 剩余变量的维度
    Eigen::MatrixXd Amm = 0.5 * (H_marg.block(n2, n2, m2, m2) + H_marg.block(n2, n2, m2, m2).transpose());

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd(
            (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
                              saes.eigenvectors().transpose();
cpp 复制代码 
// TODO:: home work. 完成舒尔补操作
//Eigen::MatrixXd Arm = H_marg.block(?,?,?,?);
//Eigen::MatrixXd Amr = H_marg.block(?,?,?,?);
//Eigen::MatrixXd Arr = H_marg.block(?,?,?,?);
Eigen::MatrixXd Arm = H_marg.block(0,n2,n2,m2);
Eigen::MatrixXd Amr = H_marg.block(n2,0,m2,n2);
Eigen::MatrixXd Arr = H_marg.block(0,0,n2,n2);

Eigen::MatrixXd tempB = Arm * Amm_inv;
Eigen::MatrixXd H_prior = Arr - tempB * Amr;

3 添加先验prior约束

解决H矩阵不满秩、不正定

解决办法2：添加先验约束，增加系统的可观性。g2o、ceres中对系统的第一个pose的信息矩阵加上单位阵 H [ 11 ] + = I \mathbf{H}_{[11]}+=\mathbf{I} H[11]+=I。

在代码中给第一帧和第二帧添加pior约束，并比较为prior设定不同权重时，BA求解收敛精度和速度。

待补充

4 其余部分程序

Frame

cpp 复制代码 
/*
 * Frame : 保存每帧的姿态和观测
 */
struct Frame {
    Frame(Eigen::Matrix3d R, Eigen::Vector3d t) : Rwc(R), qwc(R), twc(t) {};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;

    unordered_map<int, Eigen::Vector3d> featurePerId; // 该帧观测到的特征以及特征id
};

生成虚拟相机位姿、特征点---------代码和第四讲0海塞矩阵零空间维度验证一致

cpp 复制代码 
/*
 * 产生世界坐标系下的虚拟数据: 相机姿态, 特征点, 以及每帧观测
 */
void GetSimDataInWordFrame(vector<Frame> &cameraPoses, vector<Eigen::Vector3d> &points) {
    int featureNums = 20;  // 特征数目，假设每帧都能观测到所有的特征
    int poseNums = 3;     // 相机数目

    double radius = 8;
    for (int n = 0; n < poseNums; ++n) {
        double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        cameraPoses.push_back(Frame(R, t));
    }

    // 随机数生成三维特征点
    std::default_random_engine generator;
    std::normal_distribution<double> noise_pdf(0., 1. / 1000.);  // 2pixel / focal
    for (int j = 0; j < featureNums; ++j) {
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);
        std::uniform_real_distribution<double> z_rand(4., 8.);

        Eigen::Vector3d Pw(xy_rand(generator), xy_rand(generator), z_rand(generator));
        points.push_back(Pw);

        // 在每一帧上的观测量
        for (int i = 0; i < poseNums; ++i) {
            // Pc = Rcw*(Pw - twc) = Rcw*Pw - Rcw*twc = Rcw*Pw + tcw = Pc
            Eigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);
            Pc = Pc / Pc.z();  // 归一化图像平面
            Pc[0] += noise_pdf(generator);
            Pc[1] += noise_pdf(generator);
            cameraPoses[i].featurePerId.insert(make_pair(j, Pc));
        }
    }
}
相关推荐
一只小bit2 小时前
数据结构之栈，队列，树
c语言·开发语言·数据结构·c++
沐泽Mu3 小时前
嵌入式学习-QT-Day05
开发语言·c++·qt·学习
szuzhan.gy4 小时前
DS查找—二叉树平衡因子
数据结构·c++·算法
火云洞红孩儿4 小时前
基于AI IDE 打造快速化的游戏LUA脚本的生成系统
c++·人工智能·inscode·游戏引擎·lua·游戏开发·脚本系统
FeboReigns5 小时前
C++简明教程（4）（Hello World）
c语言·c++
FeboReigns5 小时前
C++简明教程（10）（初识类）
c语言·开发语言·c++
zh路西法5 小时前
【C++决策和状态管理】从状态模式，有限状态机，行为树到决策树(二)：从FSM开始的2D游戏角色操控底层源码编写
c++·游戏·unity·设计模式·状态模式
.Vcoistnt6 小时前
Codeforces Round 994 (Div. 2)(A-D)
数据结构·c++·算法·贪心算法·动态规划
小k_不小6 小时前
C++面试八股文：指针与引用的区别
c++·面试
沐泽Mu6 小时前
嵌入式学习-QT-Day07
c++·qt·学习·命令模式
热门推荐
01大模型国产化适配7-华为昇腾LLM落地可选解决方案（MindFormers、ModelLink、MindIE）02Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO03玄机平台应急响应—webshell查杀04电赛2024年H题智能小车基于MSPM0G3507主控MCU（利用8路灰度加上MPU6050的解决方式）具体项目报告05组基轨迹建模 GBTM的介绍与实现（Stata 或 R）06支付宝短视频平台创作分成激励项目07STM32 无法烧录08【一文读懂】NTN（非地面网络）技术介绍09优化手机性能，解决卡顿问题：关闭这3个微信开关，释放内存空间10怎么找小红书美食探店博主合作？