从零手写VIO第三次作业

1.1样例代码中LM阻尼因子为Lambda，在函数IsGoodStepInLM()中判断了当前Lambda是否使误差下降，最终函数只记录了正确的Lambda，需要将整个Lambda的迭代过程值记录下来，以下位置添加代码

if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
{
    double alpha = 1. - pow((2 * rho - 1), 3);
    alpha = std::min(alpha, 2. / 3.);
    double scaleFactor = (std::max)(1. / 3., alpha);
    currentLambda_ *= scaleFactor;	
    ni_ = 2;
    currentChi_ = tempChi;
    
	std::cout<< "Lambda:" << currentLambda_ << endl;	//记录Lambda
	
    return true;
} else {
    currentLambda_ *= ni_;
    ni_ *= 2;
    
	std::cout<< "Lambda:" << currentLambda_ << endl;	//记录Lambda
	
    return false;
}

输出Lambda，使用python画曲线，代码如下

import matplotlib.pyplot as plt

Lambda=[0.001,0.002,0.008,0.064,1.024,32.768,2097.15,699.051,1398.1,5592.41,1864.14,1242.76,414.252,138.084,46.028,15.3427,5.11423,1.70474,0.568247,0.378832]
plt.plot(Lambda)
plt.ylabel("Lambda")
plt.xlabel("迭代次数")
plt.show()

1.2主要是修改三个地方的代码
main()中

double x = i/100.;
double n = noise(generator);
// 观测 y
//double y = std::exp( a*x*x + b*x + c ) + n;
double y = a*x*x + b*x + c + n;	//作业1.2

误差模型
CurveFittingEdge中

virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        //residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_;  // 构建残差
		residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_;  // 作业1.2 构建残差
    }

    // 计算残差对变量的雅克比
    virtual void ComputeJacobians() override
    {
        Vec3 abc = verticies_[0]->Parameters();
        //double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );
        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
        //jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y;
		jaco_abc << x_ * x_, x_, 1;
        jacobians_[0] = jaco_abc;
    }