0
点赞
收藏
分享

微信扫一扫

VR全景HDR拍摄教程

萍儿的小确幸 02-29 11:30 阅读 2

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

文章目录

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

在这里插入图片描述

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

    // 所有 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 确定维度

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

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

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

在这里插入图片描述

// 统计优化变量的维数,为构建 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_

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,一个是对顶点3assert也可验证。

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

在这里插入图片描述

  实际代码中两个for循环计算的hessian6*63*36*33*6小雅可比矩阵,通过找到其在大雅可比矩阵中的索引,进行填充。

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

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

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

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


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{(JJ)ii}

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问题—求逆

    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 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

// 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}) (HppHpmHmm1Hmp)

// 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();
}

  左乘矩阵,将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}. [I0HpmHmm1I][HppHmpHpmHmm][ΔxpΔxc]=[I0HpmHmm1I][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} [(HppHpmHmm1Hmp)Hmp0Hmm][ΔxpΔxc]=[bppHpmHmm1bmmbmm]

// 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} (HppHpmHmm1Hmp)Δxp=bppHpmHmm1bmm

// 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;
③ 求解线性方程组
 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=bmHmpΔxp

// 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矩阵中边缘化的量

在这里插入图片描述

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;

在这里插入图片描述

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

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


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;
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给出来了,实质上也没有什么特别的。

    /// 开始 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();
// 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矩阵不满秩、不正定

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

待补充

4 其余部分程序

Frame

/*
 * 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
};
/*
 * 产生世界坐标系下的虚拟数据: 相机姿态, 特征点, 以及每帧观测
 */
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));
        }
    }
}
举报

相关推荐

0 条评论