折腾笔记[9]-使用rust通过图优化进行曲线拟合

摘要

使用rust通过图优化方式进行二次曲线拟合.
Using Rust to perform quadratic curve fitting through graph optimization.

关键词

rust;g2o;Graph Optimization;

关键信息

项目地址:[https://github.com/ByeIO/slambook2.rs],
配置文件:

[package]
name = "exp65-rust-ziglang-slambook2"
version = "0.1.0"
edition = "2021"

[dependencies]
env_logger = { version = "0.11.6", default-features = false, features = [
    "auto-color",
    "humantime",
] }

# 随机数
rand = "0.8.5"
rand_distr = "0.4.3"
fastrand = "2.3.0"

# 线性代数
nalgebra = { version = "0.33.2",features = ["rand"]}
ndarray = "0.16.1"

# winit
wgpu = "23.0.1"
winit = "0.30.8"

# egui
eframe = "0.30.0"
egui = { version = "0.30.0", features = [
    "default"
]}
egui_extras = {version = "0.30.0",features = ["default", "image"]}

# three_d
three-d = {path = "./static/three-d" , features=["egui-gui"] }
three-d-asset = {version = "0.9",features = ["hdr", "http"] }

# sophus
sophus = { version = "0.11.0" }
sophus_autodiff = { version = "0.11.0" }
sophus_geo = "0.11.0"
sophus_image = "0.11.0"
sophus_lie = "0.11.0"
sophus_opt = "0.11.0"
sophus_renderer = "0.11.0"
sophus_sensor = "0.11.0"
sophus_sim = "0.11.0"
sophus_spline = "0.11.0"
sophus_tensor = "0.11.0"
sophus_timeseries = "0.11.0"
sophus_viewer = "0.11.0"
tokio = "1.43.0"
approx = "0.5.1"
bytemuck = "1.21.0"
thingbuf = "0.1.6"

# rust-cv
cv = { version = "0.6.0" , features = ["default"] }
cv-core = "0.15.0"
cv-geom = "0.7.0"
cv-pinhole = "0.6.0"
akaze = "0.7.0"
eight-point = "0.8.0"
lambda-twist = "0.7.0"
image = "0.25.5"

# 最小二乘优化
gomez = "0.5.0"

# 图优化
factrs = "0.2.0"

# 依赖覆盖
[patch.crates-io]
pulp = { path = "./static/pulp" }

原理简介

图优化简介

[https://www.cnblogs.com/gaoxiang12/p/5244828.html]
图优化将优化问题表示为图结构，包含顶点（优化变量）和边（观测方程）。
图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。
"图优化" 是指在图结构上进行优化计算的技术，通常用于解决涉及多个变量和约束的复杂问题。它在计算机视觉、机器人导航（如SLAM）、传感器校准等领域有广泛应用。图优化的核心思想是将问题建模为图，其中节点表示变量，边表示约束或误差项，通过优化算法（如高斯-牛顿法、列文伯格-马夸尔特法）来最小化误差，从而求解出最优的变量值。
　　边是什么呢？一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最常见的边连接两个顶点。当一个图中存在连接两个以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就可以表示成一个超图。

图优化流程

1. 选择节点与边的类型
   确定图中节点和边的类型，并选择它们的参数化形式。

   • 节点：通常表示状态变量（如位姿、路标点等）。
   • 边：表示节点之间的约束（如观测模型、运动模型等）。

2. 添加节点和边
   将实际的节点和边添加到图中，构建图结构。

3. 选择初值
   为节点变量选择初始值，作为优化的起点。

4. 迭代优化
   开始迭代优化，每一步迭代包括以下步骤：

   • 计算雅可比矩阵和海塞矩阵
     根据当前估计值，计算误差函数对状态变量的雅可比矩阵 $J$ 和海塞矩阵 $H$：

$$H=J^{T}J$$

$$b=J^{T}e$$

其中 $e$ 是误差向量。

• 求解稀疏线性方程
  求解线性方程：

$$H_k\mathrm{\Delta }x=-b_k$$

得到梯度方向 $\mathrm{\Delta }x$。

• 更新状态变量
  使用高斯-牛顿（GN）或列文伯格-马夸特（LM）方法更新状态变量：

$$x_{k+1}=x_k+\mathrm{\Delta }x$$

5. 判断收敛
   如果迭代收敛（如误差小于阈值或达到最大迭代次数），返回优化值；否则继续迭代。

g2o库简介

[https://openslam-org.github.io/g2o.html]
[https://github.com/RainerKuemmerle/g2o]
[https://github.com/RainerKuemmerle/g2o/releases/download/20241228_git/g2o.pdf]
g2o 是一个开源的 C++ 框架，用于优化基于图的非线性误差函数。g2o 的设计目标是使其能够轻松扩展到各种问题，通常只需几行代码即可指定一个新问题。当前的实现提供了对多种 SLAM（同步定位与地图构建）和 BA（捆绑调整）变体的解决方案。

在机器人学和计算机视觉领域，许多问题都涉及最小化可以表示为图的非线性误差函数。典型的例子包括同步定位与地图构建（SLAM）或捆绑调整（BA）。这些问题的总体目标是找到能够最大程度解释一组受高斯噪声影响的测量值的参数配置或状态变量。g2o 是一个针对此类非线性最小二乘问题的开源 C++ 框架。g2o 的设计使其能够轻松扩展到各种问题，通常只需几行代码即可指定一个新问题。当前的实现提供了对多种 SLAM 和 BA 变体的解决方案。g2o 的性能可与针对特定问题的最先进方法实现相媲美（截至 2011 年 2 月）。

g2o is an open-source C++ framework for optimizing graph-based nonlinear error functions. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA.
A wide range of problems in robotics as well as in computer-vision involve the minimization of a non-linear error function that can be represented as a graph. Typical instances are simultaneous localization and mapping (SLAM) or bundle adjustment (BA). The overall goal in these problems is to find the configuration of parameters or state variables that maximally explain a set of measurements affected by Gaussian noise. g2o is an open-source C++ framework for such nonlinear least squares problems. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA. g2o offers a performance comparable to implementations of state-of-the-art approaches for the specific problems (02/2011).

• 参考文档: g2o: A General Framework for Graph Optimization
  g2o 可以将问题嵌入为图或超图进行优化，广泛应用于机器人学和计算机视觉领域，例如 SLAM 和 Bundle Adjustment。
  g2o 的核心优势：

• 通用性强： 可以轻松应用于各种问题，并提供易于扩展和使用的库。
• 易于理解： 提供了易于阅读的实现代码，帮助用户理解 SLAM 和 BA 的细节。
• 性能优越： 在保持通用性的同时，实现了最先进的性能。
  文档内容概述：

1. (超)图嵌入优化问题： 介绍了最小二乘问题的数学表达，以及如何将其表示为图或超图结构。
2. 最小二乘优化： 解释了 Gauss-Newton 和 Levenberg-Marquardt 算法，并分析了线性化系统的结构。
3. 流形上的最小二乘： 讨论了在非欧几里得空间进行优化的方法，并介绍了流形和 ⊞ 运算符的概念。
4. 鲁棒最小二乘： 介绍了 Huber 成本函数，以提高算法对异常值的鲁棒性。
5. 库概述： 介绍了 g2o 的高层行为、基本结构和关键类，例如 OptimizableGraph、Vertex、Edge 和 Solver。
6. 线性化问题的构建和表示： 详细描述了初始化、计算误差、线性化、构建线性系统和更新 Levenberg-Marquardt 惩罚因子的步骤。
7. 求解器： 解释了求解线性系统的重要性，并介绍了 g2o 支持的几种求解器，例如 PCG 和 CHOLMOD。
8. 操作： 介绍了 g2o 的插件架构，允许用户在运行时加载类型和求解器。
9. g2o 工具： 介绍了 g2o 提供的两个工具：命令行界面和图形用户界面，方便用户进行优化和可视化。
10. 2D SLAM 示例： 以 2D SLAM 为例，详细展示了如何使用 g2o 实现优化问题，包括状态变量、约束建模、参数化选择和误差函数设计。
    总结：
    g2o 是一个功能强大且易于使用的非线性最小二乘优化框架，适用于机器人学和计算机视觉领域的各种问题。

最小二乘优化简介

[https://zh.wikipedia.org/wiki/最小二乘法]
[https://blog.csdn.net/wydbyxr/article/details/83212881]
最小二乘法的目标是“求误差的最小平方和”。
最小二乘的分类有两种：线性和非线性:

1. 线性最小二乘的解是 closed-form（闭式），即 $x=(A^{T}A{)}^{-1}{A}^{T}b$；
2. 非线性最小二乘没有 closed-form，通常用迭代法求解。
   迭代法
   即在每一步 update 未知量逐渐逼近解，可以用于各种各样的问题（包括最小二乘），比如求的不是误差的最小平方和而是最小立方和。

梯度下降

梯度下降是迭代法的一种，可以用于求解最小二乘问题（线性和非线性都可以）。关键是学习率的选择。

高斯-牛顿法

高斯-牛顿法是另一种经常用于求解非线性最小二乘的迭代法（一定程度上可视为标准非线性最小二乘求解方法）。使用函数 $f(x)$ 的泰勒级数的前面几项来寻找方程 $f(x)=0$ 的根。牛顿法最大的特点就在于它的收敛速度很快（特别是对于二阶函数(考虑多元函数的话要在凸函数的情况下)，牛顿法能够一步到达，非常有效）。
解决约束优化问题可用拉格朗日乘数法。

fact.rs库简介

[https://github.com/rpl-cmu/fact.rs]
fact.rs（发音为 "factors"）是一个基于因子图的非线性最小二乘优化库，使用 Rust 语言编写。

它专门面向机器人学中的传感器融合问题，旨在实现快速、易用和安全。fact.rs 的 API 深受 gtsam 库的启发。

目前，它支持以下功能：

• 高斯-牛顿和列文伯格-马夸特优化器
• 支持常见的李群（SO2、SO3、SE2、SE3），并在李代数中进行优化
• 通过对偶数实现自动微分
• 通过可选的 serde 支持，实现图和变量的序列化
• 轻松转换为 rerun 类型，便于直接可视化

我们建议您查看文档以获取更多信息。要使用 fact.rs，只需将其添加到您的 Cargo.toml 中即可开始使用！

fact.rs (pronounced factors) is a nonlinear least squares optimization library over factor graphs written in Rust.

It is specifically geared toward sensor fusion in robotics. It aims to be fast, easy to use, and safe. The fact.rs API takes heavy inspiration from the gtsam library.

Currently, it supports the following features

Gauss-Newton & Levenberg-Marquadt Optimizers
Common Lie Groups supported (SO2, SO3, SE2, SE3) with optimization in Lie Algebras
Automatic differentiation via dual numbers
Serialization of graphs & variables via optional serde support
Easy conversion to rerun types for straightforward visualization
We recommend you checkout the docs for more info. For usage, simply add factrs to your Cargo.toml and start using it!

因子图(Factor Graph)简介

[https://longaspire.github.io/blog/因子图介绍/]
[https://www.zhangzhenhu.com/probability_model/6.因子图_lecture_4.html]
[https://www.cnblogs.com/TMatrix52/p/7749984.html]
有向图和无向图都使得若干个变量的一个全局函数(联合概率分布)表示为这些变量子集上的因子的乘积。 因子图显示的表示出了这个分解，方法是：在表示变量的结点的基础上，引入额外的结点表示因子本身。 因子图能够用来表达上述传统的有向和无向图模型无法表达的结构。

实现

原始cpp代码:[https://github.com/gaoxiang12/slambook2/blob/master/ch6/g2oCurveFitting.cpp]

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
  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));
  }

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);
  }

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  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;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

使用rust重构:

#![allow(dead_code)]
#![allow(unused_variables)]
#![allow(unused_imports)]
#![allow(non_fmt_panics)]
#![allow(unused_mut)]
#![allow(unused_assignments)]
#![cfg_attr(not(debug_assertions), windows_subsystem = "windows")]
#![allow(rustdoc::missing_crate_level_docs)]
#![allow(unsafe_code)]
#![allow(clippy::undocumented_unsafe_blocks)]
#![allow(unused_must_use)]
#![allow(non_snake_case)]

// 线性代数
use nalgebra::{DMatrix, DVector, Matrix3, Vector3, SVector, Matrix, VecStorage, Dyn};

// 图优化
use factrs::{
    assign_symbols,
    core::{BetweenResidual, GaussNewton, Graph, Values},
    dtype, fac,
    linalg::{Const, ForwardProp, Numeric, NumericalDiff, VectorX, DiffResult, MatrixX},
    residuals::Residual1,
    traits::*,
    variables::{VectorVar2, SE2, VectorVar3},
    containers::Key,
};

use rand::{Rng, SeedableRng};
use rand::distributions::{Distribution, Uniform};

use std::process::exit;
use std::ops::Add;
use std::ops::Mul;

// 曲线拟合的因子
#[derive(Clone, Debug)]
pub struct CurveFittingFactor {
    x: f64,
    measurement: f64,
}

impl CurveFittingFactor {
    pub fn new(x: f64, measurement: f64) -> Self {
        Self { x, measurement }
    }
}

// 实现 Residual1 trait 用于因子
// `mark` 宏处理序列化内容以及一些自定义实现
#[factrs::mark]
impl Residual1 for CurveFittingFactor {
    type Differ = ForwardProp<<Self as Residual1>::DimIn>;
    type V1 = VectorVar3;
    type DimIn = Const<3>;
    type DimOut = Const<1>;

    // residual1_jacobian要放在residual1之前
    // fn residual1_jacobian(&self, values: &Values, keys: &[Key]) -> DiffResult<VectorX, MatrixX> {
    //     let abc: &VectorVar2 = values
    //         .get_unchecked(keys[0])
    //         .expect("got wrong variable type");
    //     let y = (abc[0] * self.x * self.x + abc[1] * self.x + abc[2]).exp();
    //     let jacobian = MatrixX::from_row_slice(1, 3, &[-self.x * self.x * y, -self.x * y, -y]);
    //     DiffResult {
    //         value: self.residual1(abc.clone()),
    //         diff: jacobian,
    //     }
    // }

    fn residual1<T: Numeric>(&self, v: VectorVar3<T>) -> VectorX<T> {
        let abc = v.to_owned();
        let x_squared = T::from_f64(self.x * self.x).unwrap_or_else(|| T::from_f64(0.0).unwrap());
        let x = T::from_f64(self.x).unwrap_or_else(|| T::from_f64(0.0).unwrap());
        let measurement = T::from_f64(self.measurement).unwrap_or_else(|| T::from_f64(0.0).unwrap());
        println!("abc: {:#?}", abc);
        let error = measurement - (abc[0] * x_squared + abc[1] * x + abc[2]).exp();
        // 返回值
        VectorX::from_column_slice(&[error])
    }

}

// 定义符号变量
assign_symbols!(X: VectorVar3);

fn main() {
    // 真实参数值
    let ar = 1.0;
    let br = 2.0;
    let cr = 1.0;
    // 估计参数值
    let ae = 2.0;
    let be = -1.0;
    let ce = 5.0;
    // 数据点
    let N = 100;
    // 噪声Sigma值
    let w_sigma = 1.0;
    // 计算精度
    let inv_sigma = 1.0 / w_sigma;

    // 随机数产生器
    let mut rng = rand::rngs::StdRng::seed_from_u64(42);
    let distribution = Uniform::new(-1.0, 1.0);

    let mut x_data = Vec::new();
    let mut y_data = Vec::new();

    for i in 0..N {
        let x = i as f64 / 100.0;
        x_data.push(x);
        y_data.push((ar * x * x + br * x + cr).exp() + rng.sample(distribution) * w_sigma * w_sigma);
    }

    // 构建图优化
    let mut graph = Graph::new();

    // 添加因子
    for i in 0..N {
        let factor = CurveFittingFactor::new(x_data[i], y_data[i]);
        let factor_node = fac![factor, X(0), inv_sigma as std];
        graph.add_factor(factor_node);
    }

    // 创建初始值
    let mut values = Values::new();
    values.insert(X(0), VectorVar3::new(ae, be, ce));

    // 优化
    let mut opt: GaussNewton = GaussNewton::new(graph);
    let result = opt.optimize(values).expect("优化失败");

    println!("最终结果: {:#?}", result);
}

python代码:

import numpy as np
import scipy.optimize as opt
import matplotlib.pyplot as plt
import time

# 定义曲线模型
def curve_model(params, x):
    a, b, c = params
    return np.exp(a * x**2 + b * x + c)

# 定义误差函数
def error_function(params, x_data, y_data):
    return y_data - curve_model(params, x_data)

# 生成数据
def generate_data(a_true, b_true, c_true, N, w_sigma):
    np.random.seed(0)
    x_data = np.linspace(0, 1, N)
    y_data = curve_model([a_true, b_true, c_true], x_data) + np.random.normal(0, w_sigma, N)
    return x_data, y_data

# 主函数
def main():
    # 真实参数值
    a_true, b_true, c_true = 1.0, 2.0, 1.0
    # 估计参数值
    ae, be, ce = 2.0, -1.0, 5.0
    # 数据点数量
    N = 100
    # 噪声Sigma值
    w_sigma = 1.0

    # 生成数据
    x_data, y_data = generate_data(a_true, b_true, c_true, N, w_sigma)

    # 初始参数估计
    params_initial = np.array([ae, be, ce])

    # 优化
    print("Start optimization")
    start_time = time.time()
    result = opt.least_squares(error_function, params_initial, args=(x_data, y_data), method='lm')
    end_time = time.time()
    print(f"Solve time cost = {end_time - start_time} seconds.")

    # 输出优化值
    a_est, b_est, c_est = result.x
    print(f"Estimated model: a = {a_est}, b = {b_est}, c = {c_est}")

    # 可视化结果
    plt.scatter(x_data, y_data, label='Data points')
    x_fit = np.linspace(0, 1, 100)
    y_fit = curve_model([a_est, b_est, c_est], x_fit)
    plt.plot(x_fit, y_fit, 'r', label='Fitted curve')
    plt.legend()
    plt.xlabel('x')
    plt.ylabel('y')
    plt.title('Curve Fitting')
    plt.show()

if __name__ == "__main__":
    main()

结果

最终结果: Values {
    X0: VectorVar3(0.976, 2.052, 0.976),
}

分析

根据提供的C++代码逻辑和Rust、Python代码的结果，我们可以进行以下分析：

C++代码逻辑

• 目标：使用g2o库进行曲线拟合，通过优化估计参数 (a), (b), (c)。
• 数据生成：生成带噪声的二次曲线数据。
• 优化过程：
  • 使用高斯-牛顿法进行优化。
  • 初始化参数为 (a=2.0), (b=-1.0), (c=5.0)。
  • 进行10次迭代优化。
• 输出：优化后的参数估计值。

Rust代码结果

• 初始参数：(a=2.0), (b=-1.0), (c=5.0)。
• 结果：输出的参数值没有变化，仍然是初始值。这表明优化过程可能没有正确执行，或者优化算法没有收敛。

Python代码结果

• 结果：优化后的参数为 (a=1.3229), (b=1.5850), (c=1.1154)。
• 说明：Python代码成功进行了优化，参数值接近真实值 (a=1.0), (b=2.0), (c=1.0)。

分析

• C++和Python：C++和Python代码都成功进行了优化，Python代码的结果表明优化过程有效。
• Rust代码问题：Rust代码的结果没有变化，可能是因为：
  • 优化算法没有正确实现。
  • 数据生成或误差计算有问题。
  • 需要检查优化算法的实现细节，确保其能够正确更新参数。

建议

• 检查Rust代码：确保优化算法的实现正确，检查误差计算和雅可比矩阵的计算。
• 调试信息：添加更多调试信息，查看每次迭代的参数变化。
• 对比实现：对比C++和Python代码的实现，找出Rust代码中的差异。

根据提供的C++代码逻辑，我们可以分析Rust和Python代码的结果。C++代码的主要目标是使用g2o库进行曲线拟合优化，拟合的目标函数是一个二次指数函数，形式为：

[ y = \exp(a x^2 + b x + c) ]

其中，a, b, c 是需要优化的参数。

C++代码逻辑分析

1. 数据生成：

   • 使用真实参数 ar = 1.0, br = 2.0, cr = 1.0 生成100个数据点。
   • 每个数据点的 y 值是通过真实函数加上高斯噪声生成的。

2. 优化过程：

   • 使用Gauss-Newton算法进行优化。
   • 优化变量是 a, b, c，初始估计值为 ae = 2.0, be = -1.0, ce = 5.0。
   • 优化目标是使误差最小化，误差定义为观测值与模型预测值之间的差异。

3. 输出结果：

   • 优化后的参数估计值 abc_estimate。

Rust代码结果分析

Rust代码的结果是：

最终结果: Values {
    X0: VectorVar3(0.976, 2.052, 0.976),
}

• 这个结果表示优化后的参数估计值为 a = 0.976, b = 2.052, c = 0.976。
• 与C++代码的真实参数 ar = 1.0, br = 2.0, cr = 1.0 相比，Rust的结果非常接近真实值，说明Rust代码的优化过程是有效的。

Python代码结果分析

Python代码的结果是：

Start optimization
Solve time cost = 0.0004918575286865234 seconds.
Estimated model: a = 1.3229447793471707, b = 1.584999887555887, c = 1.1154403197313632

• 这个结果表示优化后的参数估计值为 a = 1.3229, b = 1.5850, c = 1.1154。
• 与C++代码的真实参数相比，Python的结果与真实值有一定的偏差，尤其是 a 和 b 的估计值与真实值相差较大。

结果对比与结论

1. Rust结果：

   • Rust的结果非常接近真实值，说明Rust代码的优化过程是准确的，且可能使用了与C++代码相似的优化算法和参数设置。

2. Python结果：

   • Python的结果与真实值有一定的偏差，可能的原因包括：
     • 优化算法的实现不同（例如，Python可能使用了不同的优化库或算法）。
     • 初始参数设置不同。
     • 数据生成或噪声处理方式不同。
     • 优化迭代次数或收敛条件不同。

3. C++代码的参考：

   • C++代码使用了Gauss-Newton算法进行优化，并且设置了10次迭代。Rust的结果与C++的真实参数非常接近，说明Rust可能也使用了类似的优化算法和迭代次数。
   • Python的结果偏差较大，可能是因为优化算法或参数设置与C++代码不一致。

总结

• Rust代码的结果与C++代码的真实参数非常接近，说明Rust的优化过程是有效的。
• Python代码的结果与真实值有一定的偏差，可能需要检查优化算法、初始参数设置或数据生成过程是否与C++代码一致。