从0开始学习Ceres NO.3

感天动地大白狗

已于 2023-04-24 22:13:40 修改

阅读量355

点赞数

分类专栏： SLAM 文章标签： slam

于 2022-04-03 20:20:10 首次发布

原文链接：http://ceres-solver.org/nnls_tutorial.html#powell-s-function

版权

SLAM 专栏收录该内容

18 篇文章 2 订阅

订阅专栏

上一篇文章从0开始学习Ceres NO.2学习了CostFunction的另外两种写法。这次我们从简单的 $\frac{1}{2}(10-x)^2$ 扩展，看看别的情况下应该如何用Ceres解决问题。

Powel’s Function

第一个例子为求解鲍威尔方程(Powel’s Function)的最小值。对于输入参数块 $x=[x_1, x_2, x_3, x_4]$ ，有方程:
$\begin{aligned} & f_1(x) = x_1 + 10x_2\\ & f_2(x) = \sqrt{5}(x_3-x_4)\\ & f_3(x) = (x_2 - 2_3)^2\\ & f_4(x) = \sqrt{10}(x_1-x_4)^2\\ & F(x) = [f_1(x), f_2(x), f_3(x), f_4(x)] \end{aligned}$
我们的优化问题为：
$\underset{x}min \quad \frac{1}{2}\left\|F(x)\right\|^2$

问题的解决

同样的步骤，首先我们定义CostFuction，我们采用自动求导的形式。 $f_1(x)$ 的结构体形式如下，对于另外三个 $f$ 以同样的方式定义。

struct F1{
    template<typename T>
    bool operator()(const T* const x1, const T* const x2, T* residual) const{
        residual[0] = x1[0] + 10.0*x2[0];
        return true;
    }
};

主函数如下，和之前的主要框架并无二致，只不过将四个函数分别通过AddResidualBlock添加到problem类中

int main(){
    double x1 = 3.0, x2=-1.0, x3=0.0, x4=1.0;

    ceres::Problem problem;

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<F1, 1, 1, 1>(new F1), nullptr, &x1, &x2
    );
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<F2, 1, 1, 1>(new F2), nullptr, &x3, &x4
    );
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<F3, 1, 1, 1>(new F3), nullptr, &x2, &x3
    );
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<F4, 1, 1, 1>(new F4), nullptr, &x1, &x4
    );

    ceres::Solver::Options options;
    options.max_num_iterations = 100;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;

    cout << "Initial x1 = " << x1
            << ", x2 = " << x2
            << ", x3 = " << x3
            << ", x4 = " << x4 << endl;
    
    ceres::Solve(options, &problem, &summary);

    cout << "Final x1 = " << x1
            << ", x2 = " << x2
            << ", x3 = " << x3
            << ", x4 = " << x4 << endl;
    
    return 0;
}

运行结果

正确的结果应该为 $x_1=0, x_2=0, x_3=0, x_4=0]$ ，ceres迭代14次完成计算的结果为 $x_1=0.000146222, x_2=-0.0000146222, x_3=0.0000240957, x_4=0.0000240957]$ ，可以算是准确的
请添加图片描述

曲线拟合

之前的例子都是在我们确定一个函数形式 $F (x)$ 的情况下对其求最小值的问题。当我们拥有一堆数据并希望利用这些数据去找出最能描述他们的曲线时，我们拥有的只有一堆点的坐标，此时我们需要自己构造 $F (x)$ 。
假设我们有曲线 $y_{true}=e^{0.3x+0.1}$ 的带高斯噪声的点分布，并且我们知道曲线的形式是 $y_{guess}=e^{mx+c}$ ，此时我们需要做的就是取寻找最合适的m和c，只需构造优化问题如下：
$\underset{x}min \quad \sum_{i=1}^{N}\left\|y_{true}-y_{guess}\right\|^2$

问题的解决

1、定义CostFunction，采用自动求导方式。需要注意的是，此时operator中传入的参数是m和c，可以发现传入的即为需要优化的参数，本问题中x和y只是结果值并不是需要优化的值，所以是在构造结构体的时候传入的，这一点需要拎清。

struct ExponentialResidual{
    ExponentialResidual(double x, double y): x_(x), y_(y){}
    template<typename T>

    bool operator()(const T* const m, const T* const c, T* residual) const{
        residual[0] = y_ - exp(m[0]*x_+c[0]);
        return true;
    }
    
    private:
        const double x_;
        const double y_;
};

2、构造原始数据，代码中的M，N，R是我定义的全局变量，可以自己改成希望的值，分别表示曲线的两个参数和高斯噪声的方差

void cerateData(double* data, int kNumObservations=20){
    double tmp[kNumObservations];
    unsigned seed = chrono::system_clock::now().time_since_epoch().count();
    default_random_engine generator(seed);
    normal_distribution<double> distribution(0.0, R);
    for(int i=0; i<kNumObservations; i++)
        data[2*i] = 0.1*i;
    for(int i=0; i<kNumObservations; i++)
    {
        tmp[i] = exp(M*data[2*i]+C);
        data[2*i+1] = tmp[i] + distribution(generator);
    }
}

3、主函数

int main(){
    double m = 0.0;
    double c = 0.0;
    int kNumObservations = 50;
    double data[2*kNumObservations];
    cerateData(data, kNumObservations);

    ceres::Problem problem;
    for(int i=0; i<kNumObservations; i++)
    {
        ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
                new ExponentialResidual(data[2*i], data[2*i+1]));
        problem.AddResidualBlock(cost_function, nullptr, &m, &c);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;

    cout << "Initial m = " << m
            << ", c = " << c << endl;
    ceres::Solve(options, &problem, &summary);
    cout << "Final m = " << m
        << ", c = " << c << endl;
    
    return 0;
}