gtsam优化不能有效利用历史数据

Question

想使用gtsam进行非线性优化Vector1，发现并不能利用历史数据。
代码如下：

#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <GeographicLib/Geoid.hpp>
#include <math.h>
#include <vector>
#include <iostream>
#include <ctime>
#include<stdlib.h> 
#include<time.h> 
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <random>
#include <Eigen/Dense>
using namespace std;
using namespace gtsam;
using namespace boost;
class UnaryFactor : public gtsam::NoiseModelFactor1<gtsam::Vector1>
{
    double world;
public:
    using NoiseModelFactor1<gtsam::Vector1>::evaluateError;
    typedef std::shared_ptr<UnaryFactor> shared_ptr;
    gtsam::Vector1 anchor;
    UnaryFactor(gtsam::Key j, const SharedNoiseModel &model, double p)
        : NoiseModelFactor1<gtsam::Vector1>(model, j), world(p) {}

    virtual ~UnaryFactor() {}
    gtsam::Vector evaluateError(const Vector1 &anchor, boost::optional<Matrix &> H = boost::none) const
    {
        gtsam::Vector1 residual;
        gtsam::Matrix Jacobin = gtsam::Matrix::Zero(1, 1);
        double r;
        r = world + anchor[0];
        residual << r;
        Jacobin << 1;
        if (H)
        {
            (*H) = Jacobin;
        }
        return residual;
    };
};
NonlinearFactorGraph graph;
gtsam::Values initialyaw;
ISAM2Params parameters;
ISAM2 isam(parameters);
using gtsam::symbol_shorthand::X;
void optimizeGraph(int &k)
{
    const double sig = 0.1;
    std::random_device rd;
    std::default_random_engine generator_(rd());
    std::normal_distribution<double> noise(0, sig);
    
    double jjj2 = k + 10 + noise(generator_);
    double world=k - jjj2;
    cout << "world:" << world << endl;
    graph.emplace_shared<UnaryFactor>(X(k), noiseModel::Isotropic::Sigma(1, sig), world);
    initialyaw.insert(X(k),Vector1(8));
}
int main()
{
    for (int k = 0; k < 20; ++k)
    {
        optimizeGraph(k);
        cout << "k:" << k << endl;
    }
   isam.update(graph, initialyaw);
    isam.update();
    Values isamCurrentEstimate = isam.calculateEstimate();
    gtsam::Vector1 matX0 = isamCurrentEstimate.at<Vector1>(X(1));
    cout << "Result for X: " << matX0[0] << endl;
    initialyaw.clear();
    return 0;
}

anchor真实值应为10，每次优化的时候，只使用了单次优化的数据。（isam优化器和LM 高斯牛顿都尝试过）
目前想要实现的是，第K次优化，优化的数据是1-K次的数据，循环写入尝试过，有没有其他更好的方法

Answer 1

从你提供的代码来看，你正在尝试使用gtsam库来实现一个非线性的优化问题，并尝试使用历史数据。然而，你并没有提供足够的信息来展示如何使用历史数据。你的代码中只包含了一个UnaryFactor类的定义，这个类继承自gtsam::NoiseModelFactor1，这是一个一元因子，通常用于表示单个的测量模型。

在gtsam中，非线性优化通常是通过构建一个非线性因子图并使用优化器（如Gauss-Newton或Levenberg-Marquardt）来对它进行优化的。优化的目标是使得因子图的残差尽可能小。这些因子图中的每个因子都与一个或多个变量相关联，并且通过误差模型来表示测量的预测值和实际值之间的差异。

对于历史数据的利用，这通常意味着在构建因子图时，将先前估计的值作为先验信息加入到图中。这可以通过使用gtsam::PriorFactor或者gtsam::BetweenFactor等来实现。