非线性优化高翔

最新推荐文章于 2023-03-23 19:18:25 发布

荆黎明

最新推荐文章于 2023-03-23 19:18:25 发布

阅读量272

点赞数

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

本文链接：https://blog.csdn.net/jlm7689235/article/details/107447873

版权

slam 专栏收录该内容

18 篇文章 0 订阅

订阅专栏

实践部分

手写高斯牛顿法

在这里插入图片描述

高斯牛顿法

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>

using namespace std;

int main() {
    double ar = 1.0, br = 2.0, cr = 1.0 ;   //真实参数值
    double ae = 3.0, be = -2.0, ce = 1.0;   //估计参数值
    int N = 100;                            //数据点
    double w_sigma = 1.0;                   // 噪声Sigma值
    double inv_sigma = 1.0/w_sigma;
    cv::RNG rng;                            //OpenCV随机数产生器

    vector<double> x_data, y_data;         //数据(真实数据)
  for(int i = 0; i < N; i++)
    {
        double x = i/100.0;
        x_data.push_back(x);

        // rng.gaussian(w_sigma * w_sigma)
        y_data.push_back(exp(ar * x * x + br * x + cr) +
                         rng.gaussian(w_sigma * w_sigma));
    }

    // 开始Gauss-Newton 迭代
    int iterations = 100;   // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for (int iter = 0; iter < iterations; iter++)
    {
        // Hession = J^T W^{-1} J in Gauss-Newton
        Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
        // bias
        Eigen::Vector3d b = Eigen::Vector3d::Zero();
        cost = 0;

        for (int i = 0; i < N; i++)
        {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            double error = yi - exp(ae * xi * xi + be * xi + ce); // 误差
            Eigen::Vector3d J; //雅克比矩阵
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);      // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);       //de/dc

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += -inv_sigma * inv_sigma * error * J;
            cost += error * error;
        }

        // 求解线性方程　Ｈx = b
        Eigen::Vector3d dx = H.ldlt().solve(b);
        if( isnan(dx[0])){
            cout << " result is nan!" << endl;
            break；
        }

        if(iter > 0 && cost >= lastCost){
            cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << ", \t\tupdata: " << dx.transpose() <<
            "\t\testimated params: " << ae << "," << be << "," << ce << endl;
    }

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
    cout << "estimated abc = " << ae << " , " << be << ", " << ce << endl;
    return 0;
}

cmake_minimum_required(VERSION 3.16)
project(gaussNewton)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(gaussNewton main.cpp)

target_link_libraries(gaussNewton ${OpenCV_LIBRARIES})

使用Ceres进行优化

这个代码没有看明白，日后再来！！！ 参考博客

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
    CURVE_FITTING_COST(double x, double y): _x(x), _y(y) {}
    //残差的计算
    template<typename T>
    bool operator()(
    const T * const abc, // 模型参数，有３维
    T *residual) const {
        // y- exp(ax^2 + bx + c)
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
        return true;
    }
    const double _x, _y;    // x,y数据
};

int main() {
    double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;   // 估计参数值
    int N = 100;        // 数据点
    double w_sigma =  1.0;  // 噪声Sigma值
    double inv_sigma = 1.0/ w_sigma;
    cv::RNG rng;    // OpenCV 随机数产生器

    vector<double> x_data, y_data;  // 数据
    for (int i = 0; i < N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }

    double abc[3] = {ae, be, ce};

    // 构建最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < N; i++)
    {
        problem.AddResidualBlock(//向问题中添加误差项
        //　使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
        new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
                new CURVE_FITTING_COST(x_data[i], y_data[i])
                ),
        nullptr,    // 核函数，这里不使用，为空
        abc     // 待估计参数
        );
    }
    // 配置求解器
    ceres::Solver::Options options;  // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; //增量方程如何求解
    options.minimizer_progress_to_stdout = true;    //输出到cout

    ceres::Solver::Summary summary;     // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    //　输出结果
    cout << summary.BriefReport() << endl;
    cout << "estimated a, b, c = ";
    for (auto a:abc) cout << a << " ";
    cout << endl;
    return 0;
}

cmake_minimum_required(VERSION 3.16)
project(ceresCurveFitting)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Ceres REQUIRED)

include_directories(${CERES_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(ceresCurveFitting main.cpp)

target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})