深蓝学院《从零开始手写VIO》作业三

深蓝学院《从零开始手写VIO》作业三

1. 代码修改

样例代码给出了使用 LM 算法来估计曲线 y = exp(ax2 + bx + c)参数 a, b, c 的完整过程。
1 请绘制样例代码中 LM 阻尼因子 µ 随着迭代变化的曲线图
2 将曲线函数改成 y = ax2 + bx + c,请修改样例代码中残差计算,雅克比计算等函数,完成曲线参数估计。
3 如果有实现其他阻尼因子更新策略可加分(选做)

(1)阻尼因子随着迭代变化的曲线图如下:
在这里插入图片描述

(2)代码修改如下:
残差计算:

    virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_;
    }

雅克比计算:

    virtual void ComputeJacobians() override
    {
        Vec3 abc = verticies_[0]->Parameters();

        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维,状态量 3 个,所以是 1x3 的雅克比矩阵
        jaco_abc << x_ * x_, x_ , 1;
        jacobians_[0] = jaco_abc;
    }

计算结果如下(为了达到良好的迭代效果,将数据点从100个调整到1000个):

Test CurveFitting start...
iter: 0 , chi= 3.21386e+06 , Lambda= 19.95
iter: 1 , chi= 974.658 , Lambda= 6.65001
iter: 2 , chi= 973.881 , Lambda= 2.21667
iter: 3 , chi= 973.88 , Lambda= 1.47778
problem solve cost: 35.4232 ms
   makeHessian cost: 26.0736 ms
-------After optimization, we got these parameters :
0.999588   2.0063 0.968786
-------ground truth: 
1.0,  2.0,  1.0

(3)尝试阻尼因子更新策略如下:  if  ρ > 0 μ : = μ ∗ max ⁡ { 1 3 , 1 − ( 2 ρ − 1 ) 3 } ; ν : = 2  else  μ : = μ ∗ ν ; ν : = 2 ∗ ν \begin{array}{l}{\text { if } \rho>0} \\ {\qquad \mu :=\mu * \max \left\{\frac{1}{3}, 1-(2 \rho-1)^{3}\right\} ; \quad \nu :=2} \\ {\text { else }} \\ {\qquad \mu :=\mu * \nu ; \quad \nu :=2 * \nu}\end{array}  if ρ>0μ:=μmax{31,1(2ρ1)3};ν:=2 else μ:=μν;ν:=2ν
修改阻尼因子更新代码如下:

bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double rho = (currentChi_ - tempChi) / scale;
    if (rho > 0.75 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        currentLambda_ *= 1/3;
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ *= 2;
        currentChi_ = tempChi;
        return false;
    }
}

结果如下:

Test CurveFitting start...
iter: 0 , chi= 3.21386e+06 , Lambda= 19.95
iter: 1 , chi= 974.658 , Lambda= 0
iter: 2 , chi= 973.88 , Lambda= 0
problem solve cost: 27.2341 ms
   makeHessian cost: 20.6501 ms
-------After optimization, we got these parameters :
0.999589  2.00628 0.968821
-------ground truth: 
1.0,  2.0,  1.0

2. 公式推导

公式推导,根据课程知识,完成 F, G 中如下两项的推导过程:
f 15 = ∂ α b i b k + 1 ∂ δ b k g = − 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( − δ t ) \mathbf{f}_{15}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)(-\delta t) f15=δbkgαbibk+1=41(Rbibk+1[(abk+1bka)]×δt2)(δt) g 12 = ∂ α b i b k + 1 ∂ n k g = − 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( 1 2 δ t ) \mathbf{g}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)\left(\frac{1}{2} \delta t\right) g12=nkgαbibk+1=41(Rbibk+1[(abk+1bka)]×δt2)(21δt)

说明:这里的公式推导约定保持和PPT中的一样的,比如求导公式: ∂ x a ∂ δ θ = lim ⁡ δ θ → 0 R a b exp ⁡ ( [ [ δ θ ] x ) x b − R a b x b δ θ \frac{\partial \mathbf{x}_{a}}{\partial \delta \boldsymbol{\theta}}=\lim _{\delta \theta \rightarrow 0} \frac{\mathbf{R}_{a b} \exp \left(\left[[\delta \boldsymbol{\theta}]_{\mathrm{x}}\right) \mathbf{x}_{b}-\mathbf{R}_{a b} \mathbf{x}_{b}\right.}{\delta \boldsymbol{\theta}} δθxa=δθ0limδθRabexp([[δθ]x)xbRabxb后续直接简写为 ∂ x a ∂ δ θ = ∂ R a b exp ⁡ ( [ δ θ ] × ) x b ∂ δ θ \frac{\partial \mathbf{x}_{a}}{\partial \delta \boldsymbol{\theta}}=\frac{\partial \mathbf{R}_{a b} \exp \left([\delta \boldsymbol{\theta}]_{ \times}\right) \mathbf{x}_{b}}{\partial \delta \boldsymbol{\theta}} δθxa=δθRabexp([δθ]×)xb

下面开始推导

(1)公式 f 15 \mathbf{f}_{15} f15推导如下: α b i b k + 1 = α b i b k + β b i b k δ t + 1 2 a δ t 2 \boldsymbol{\alpha}_{b_{i} b_{k+1}}=\boldsymbol{\alpha}_{b_{i} b_{k}}+\boldsymbol{\beta}_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2} αbibk+1=αbibk+βbibkδt+21aδt2其中 a = 1 2 ( q b i b k ( a b k − b k a ) + q b i b k + 1 ( a b k + 1 − b k a ) ) = 1 2 ( q b i b k ( a b k − b k a ) + q b i b k ⊗ [ 1 1 2 ω δ t ] ( a b k + 1 − b k a ) ) \begin{aligned} \mathbf{a}&=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \\&=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k}}\otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \omega \delta t}\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \end{aligned} a=21(qbibk(abkbka)+qbibk+1(abk+1bka))=21(qbibk(abkbka)+qbibk[121ωδt](abk+1bka))其中 δ b k g \delta \mathbf{b}_{k}^{g} δbkg只和 1 2 ω δ t \frac{1}{2} \omega \delta t 21ωδt这一项有关,因此 f 15 \mathbf{f}_{15} f15的推导可以进行如下简化: f 15 = ∂ α b i b k + 1 ∂ δ b k g = ∂ 1 4 q b i b k ⊗ [ 1 1 2 ω δ t ] ⊗ [ 1 − 1 2 δ b k g δ t ] ( a b k + 1 − b k a ) δ t 2 ∂ δ b k g = 1 4 ∂ R b i b k + 1 exp ⁡ ( [ − δ b k g δ t ] × ) ( a b k + 1 − b k a ) δ t 2 ∂ δ b k g = 1 4 ∂ R b i b k + 1 ( I + [ − δ b k g δ t ] × ) ( a b k + 1 − b k a ) δ t 2 ∂ δ b k g = 1 4 ∂ − R b i b k + 1 ( [ ( a b k + 1 − b k a ) δ t 2 ] × ) ( − δ b k g δ t ) ∂ δ b k g = − 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( − δ t ) \begin{aligned} \mathbf{f}_{15}&=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}} \\&=\frac{\partial \frac{1}{4} \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \boldsymbol{\omega} \delta t}\end{array}\right] \otimes\left[\begin{array}{c}{1} \\ {-\frac{1}{2} \delta \mathbf{b}_{k}^{g} \delta t}\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta \mathbf{b}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}} \exp \left(\left[-\delta \mathbf{b}_{k}^{g} \delta t\right]_{ \times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta \mathbf{b}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}}\left(\mathbf{I}+\left[-\delta \mathbf{b}_{k}^{g} \delta t\right]_{ \times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta \mathbf{b}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial-\mathbf{R}_{b_{i} b_{k+1}}\left(\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}\right]_{ \times}\right)\left(-\delta \mathbf{b}_{k}^{g} \delta t\right)}{\partial \delta \mathbf{b}_{k}^{g}} \\&=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)(-\delta t) \end{aligned} f15=δbkgαbibk+1=δbkg41qbibk[121ωδt][121δbkgδt](abk+1bka)δt2=41δbkgRbibk+1exp([δbkgδt]×)(abk+1bka)δt2=41δbkgRbibk+1(I+[δbkgδt]×)(abk+1bka)δt2=41δbkgRbibk+1([(abk+1bka)δt2]×)(δbkgδt)=41(Rbibk+1[(abk+1bka)]×δt2)(δt)

(2)公式 g 12 \mathbf{g}_{12} g12推导如下:
g 12 \mathbf{g}_{12} g12 f 15 \mathbf{f}_{15} f15的推导是类似的
α b i b k + 1 = α b i b k + β b i b k δ t + 1 2 a δ t 2 \boldsymbol{\alpha}_{b_{i} b_{k+1}}=\boldsymbol{\alpha}_{b_{i} b_{k}}+\boldsymbol{\beta}_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2} αbibk+1=αbibk+βbibkδt+21aδt2其中 a = 1 2 ( q b i b k ( a b k − b k a ) + q b i b k + 1 ( a b k + 1 − b k a ) ) = 1 2 ( q b i b k ( a b k − b k a ) + q b i b k ⊗ [ 1 1 2 ω δ t ] ( a b k + 1 − b k a ) ) \begin{aligned} \mathbf{a}&=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \\&=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k}}\otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \omega \delta t}\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \end{aligned} a=21(qbibk(abkbka)+qbibk+1(abk+1bka))=21(qbibk(abkbka)+qbibk[121ωδt](abk+1bka))其中 ω = 1 2 ( ( ω b k + n k g − b k g ) + ( ω b k + 1 + n k + 1 g − b k g ) ) \omega=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}+\mathbf{n}_{k}^{g}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}+\mathbf{n}_{k+1}^{g}-\mathbf{b}_{k}^{g}\right)\right) ω=21((ωbk+nkgbkg)+(ωbk+1+nk+1gbkg))因此 n k g \mathbf{n}_{k}^{g} nkg只和 1 2 ω δ t \frac{1}{2} \omega \delta t 21ωδt这一项有关,同理: f 15 = ∂ α b i b k + 1 ∂ δ b k g = ∂ 1 4 q b i b k ⊗ [ 1 1 2 ω δ t ] ⊗ [ 1 1 2 ( 1 2 δ n k g ) δ t ] ( a b k + 1 − b k a ) δ t 2 ∂ δ n k g = 1 4 ∂ R b i b k + 1 exp ⁡ ( [ 1 2 δ n k g δ t ] × ) ( a b k + 1 − b k a ) δ t 2 ∂ δ n k g = 1 4 ∂ R b i b k + 1 ( I + [ 1 2 δ n k g δ t ] × ) ( a b k + 1 − b k a ) δ t 2 ∂ δ n k g = 1 4 ∂ − R b i b k + 1 ( [ ( a b k + 1 − b k a ) δ t 2 ] × ) ( 1 2 δ n k g δ t ) ∂ δ n k g = − 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( 1 2 δ t ) \begin{aligned} \mathbf{f}_{15}&=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}} \\&=\frac{\partial \frac{1}{4} \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \boldsymbol{\omega} \delta t}\end{array}\right] \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} (\frac{1}{2}\delta \mathbf{n}_{k}^{g} )\delta t}\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta \mathbf{n}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}} \exp \left(\left[\frac{1}{2}\delta \mathbf{n}_{k}^{g} \delta t\right]_{ \times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta \mathbf{n}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}}\left(\mathbf{I}+\left[\frac{1}{2}\delta\mathbf{n}_{k}^{g} \delta t\right]_{ \times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}}{\partial \delta\mathbf{n}_{k}^{g}} \\&=\frac{1}{4} \frac{\partial-\mathbf{R}_{b_{i} b_{k+1}}\left(\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t^{2}\right]_{ \times}\right)\left(\frac{1}{2}\delta \mathbf{n}_{k}^{g} \delta t\right)}{\partial \delta \mathbf{n}_{k}^{g}} \\&=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)(\frac{1}{2}\delta t) \end{aligned} f15=δbkgαbibk+1=δnkg41qbibk[121ωδt][121(21δnkg)δt](abk+1bka)δt2=41δnkgRbibk+1exp([21δnkgδt]×)(abk+1bka)δt2=41δnkgRbibk+1(I+[21δnkgδt]×)(abk+1bka)δt2=41δnkgRbibk+1([(abk+1bka)δt2]×)(21δnkgδt)=41(Rbibk+1[(abk+1bka)]×δt2)(21δt)


3. 公式证明:

证明公式: Δ x l m = − ∑ j = 1 n v j ⊤ F ′ ⊤ λ j + μ v j \Delta \mathbf{x}_{\mathrm{lm}}=-\sum_{j=1}^{n} \frac{\mathbf{v}_{j}^{\top} \mathbf{F}^{\prime \top}}{\lambda_{j}+\mu} \mathbf{v}_{j} Δxlm=j=1nλj+μvjFvj

证明主要是利用正规矩阵的谱分解性质,LM公式如下: ( J ⊤ J + μ I ) Δ x l m = − J ⊤ f \left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\mathrm{lm}}=-\mathbf{J}^{\top} \mathbf{f} (JJ+μI)Δxlm=Jf J ⊤ J \mathbf{J}^{\top} \mathbf{J} JJ进行特征值分解,并加以变换: ( V Λ V ⊤ + μ I ) Δ x l m = − F ′ ⊤ \left(\mathbf{V} \mathbf{\Lambda} \mathbf{V}^{\top}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\mathrm{lm}}=-\mathbf{F'}^{\top} (VΛV+μI)Δxlm=F V ( Λ + μ I ) V ⊤ Δ x l m = − F ′ ⊤ \mathbf{V}\left( \mathbf{\Lambda} +\mu \mathbf{I}\right)\mathbf{V}^{\top} \Delta \mathbf{x}_{\mathrm{lm}}=-\mathbf{F'}^{\top} V(Λ+μI)VΔxlm=F Δ x l m = − V ( Λ + μ I ) − 1 V ⊤ F ′ ⊤ \Delta \mathbf{x}_{\mathrm{lm}}=-\mathbf{V}\left( \mathbf{\Lambda} +\mu \mathbf{I}\right)^{-1}\mathbf{V}^{\top} \mathbf{F'}^{\top} Δxlm=V(Λ+μI)1VF根据正规矩阵的谱分解有: Δ x l m = − ∑ j = 1 n v j v j ⊤ λ j + μ F ′ ⊤ \Delta \mathbf{x}_{\mathrm{lm}}=-\sum_{j=1}^{n} \frac{\mathbf{v}_{j}\mathbf{v}_{j}^{\top} }{\lambda_{j}+\mu} \mathbf{F}^{\prime \top} Δxlm=j=1nλj+μvjvjF根据矩阵的结合率可以直接过得结果: Δ x 1 m = − ∑ j = 1 n v j ⊤ F ′ ⊤ λ j + μ v j \Delta \mathbf{x}_{1 \mathrm{m}}=-\sum_{j=1}^{n} \frac{\mathbf{v}_{j}^{\top} \mathbf{F}^{\prime \top}}{\lambda_{j}+\mu} \mathbf{v}_{j} Δx1m=j=1nλj+μvjFvj

<think>好的,我现在要帮助用户了解如何从零开始实现视觉惯性里程计(VIO)。首先,我需要回忆VIO的基本概念和组成部分。VIO结合了视觉(如摄像头)和惯性测量单元(IMU)的数据,用于估计设备的运动轨迹和周围环境的结构。用户提到他们想手写代码实现,所以需要从基础开始逐步构建。 根据用户提供的引用,特别是引用[1]、[3]、[4],VIO的实现涉及初始化、传感器数据融合、状态估计、后端优化等。首先,我需要确定VIO的框架,比如松耦合还是紧耦合。引用[4]提到松耦合和紧耦合的分类,而通常紧耦合效果更好,但实现更复杂。因此,可能需要从松耦合开始,逐步深入。 接下来,步骤可能包括传感器数据的同步和时间戳对齐,这在引用[3]中提到了时间戳标定的重要性。然后是IMU和视觉数据的预处理,比如IMU的零偏校正和视觉特征点的提取与跟踪。 然后是状态估计,可能需要使用滤波方法(如卡尔曼滤波)或者优化方法(如非线性最小二乘)。引用[2]提到了高斯牛顿法用于曲线拟合,这可能在后端优化中使用。因此,后端优化部分需要实现如Bundle Adjustment结合IMU数据的方法。 此外,初始化过程是关键,引用[1]和[4]都提到需要正确的初始化和外参标定。可能需要使用视觉信息来初始化IMU的参数,比如尺度、重力方向等。 最后,代码实现部分需要分模块,比如数据读取、预处理、状态预测、优化、地图管理等。可能需要使用C++或Python,但用户提供的示例代码是Python,但实际VIO系统可能更常用C++以提高效率。 在实现过程中,需要注意IMU和视觉数据的同步,时间戳的处理,以及如何处理传感器丢失的情况,如引用[3]提到的视觉丢失时IMU顶替。此外,累积误差的问题需要通过回环检测或者全局优化来解决,但可能在初始实现中先处理局部优化。 可能还需要参考现有的开源项目,如VINS-Mono或OKVIS,了解它们的结构,但用户要求手写代码,所以需要从零开始构建基本框架。 总结步骤可能为: 1. 传感器数据读取与同步 2. IMU数据预处理(去噪、零偏校正) 3. 视觉特征提取与匹配 4. 初始化和外参标定 5. 紧耦合的滤波或优化方法实现 6. 后端非线性优化(如基于高斯牛顿法或LM算法) 7. 尺度估计和重力对齐 8. 处理累积误差和闭环检测 需要确保每个模块的正确性,逐步集成测试。例如,先实现IMU的积分和预测,再处理视觉的位姿估计,然后将两者融合。可能出现的问题包括时间不同步、参数标定不准确、优化过程中的数值不稳定等,需要通过调试和测试来解决。</think>### 从零开始实现视觉惯性里程计(VIO)的步骤详解 #### 一、基础理论与框架设计 1. **VIO核心原理** VIO通过融合相机(视觉)和IMU(惯性)数据,估计载体在短时间内的相对运动。视觉提供环境特征点约束,IMU提供高频运动预测,两者互补: - IMU适合快速运动但存在零偏漂移[^3] - 视觉适合慢速运动但能校正IMU零偏并提供尺度信息[^4] 2. **系统框架选择** 推荐从**紧耦合**框架入手(直接融合原始数据),尽管实现复杂度高于松耦合,但精度更高: ```text VIO Pipeline: 传感器数据 → 时间同步 → 视觉特征提取 + IMU预积分 → 紧耦合优化 → 位姿输出 ``` --- #### 二、核心模块实现步骤 ##### 1. 传感器数据同步与预处理 - **时间戳对齐**:使用线性插值或滑动窗口对齐相机与IMU的时间戳[^1] - **IMU预处理**: - 零偏校正:通过静止状态下的均值计算初始零偏 - 角速度积分公式(四元数更新): $$ q_{t+1} = q_t \otimes \left[1, \frac{1}{2}\omega\Delta t\right]^T $$ 其中$\omega$为陀螺仪测量值 ##### 2. 视觉前端实现 ```cpp // 示例:ORB特征提取(OpenCV) cv::Ptr<cv::ORB> orb = cv::ORB::create(500); std::vector<cv::KeyPoint> kps; cv::Mat descriptors; orb->detectAndCompute(img, cv::Mat(), kps, descriptors); ``` - **光流跟踪**:使用LK光流法匹配相邻帧特征点 - **关键帧选择**:基于平移/旋转量阈值判断 ##### 3. IMU预积分 - 计算两帧图像间的IMU运动增量: $$ \Delta v = \sum_{k=i}^{j-1} (R_k a_k - g)\Delta t $$ $$ \Delta p = \sum \Delta v_k \Delta t $$ 其中$R_k$为姿态矩阵,$a_k$为加速度计测量值[^4] ##### 4. 紧耦合非线性优化 使用**滑动窗口优化**最小化视觉重投影误差与IMU预积分误差: $$ \min_{\mathbf{x}} \left( \sum \rho \left( \| r_p \|^2_{\Sigma_p} \right) + \sum \| r_I \|^2_{\Sigma_I} \right) $$ - 残差项: - 视觉残差$r_p = z - \pi(T_{cw}X)$ - IMU残差$r_I = \log\left( \Delta \hat{T}_{ij}^{-1} T_i^{-1} T_j \right)$ - 求解方法:高斯牛顿法或Levenberg-Marquardt算法[^2] --- #### 、关键问题解决方案 1. **初始化** - **视觉-IMU外参标定**:通过手眼标定法计算$T_{imu}^{cam}$ - **尺度恢复**:单目视觉通过SFM计算特征点深度,结合IMU加速度约束求解尺度因子 2. **零偏在线估计** 将加速度计和陀螺仪零偏作为状态量加入优化变量: $$ \mathbf{x} = [T_{wb}, v, b_a, b_g, \lambda_1, ..., \lambda_n] $$ 3. **关键代码示例(C++伪代码)** ```cpp class VIOOptimizer { public: void addImuResidual(const ImuData& imu) { // 添加IMU预积分残差块 problem.AddResidualBlock( new ImuCostFunction(imu.delta_p, imu.delta_v, imu.delta_q), nullptr, pose_i, pose_j, bias_a, bias_g ); } void addVisionResidual(const Feature& feat) { // 添加视觉重投影残差块 problem.AddResidualBlock( new ReprojectionCost(feat.uv_observed), new CauchyLoss(0.5), pose, landmark ); } }; ``` --- #### 四、测试与调试建议 1. **仿真数据验证** 使用EuRoC MAV数据集,对比轨迹的ATE(绝对轨迹误差) 2. **真机调试技巧** - 优先调试IMU积分:静止状态下积分速度应趋近于零 - 可视化特征跟踪:确保匹配点分布均匀且跟踪稳定 3. **性能优化方向** - 使用Schur补加速H矩阵求逆 - 引入FEJ(First Estimate Jacobians)避免线性化点不一致 ---
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值