terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'

[zhao-zhibo]

Feature

In my code, the results of initializing ros and not initializing ros are different. If ros is initialized, an exception will be thrown. If ros is not initialized, gtsam can optimize normally.
The abnormal error is as follows:
Thrown when a linear system is ill-posed. The most common cause for this error is having underconstrained variables. Mathematically, the system is underdetermined. See the GTSAM Doxygen documentation at http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for more information. [lio_sam_curveFitting-2] process has died [pid 1159573, exit code -6, cmd /home/zhao/Codes/competition_code/lio-sam-mergePoints/devel/lib/lio_sam/lio_sam_curveFitting __name:=lio_sam_curveFitting __log:=/home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2.log]. log file: /home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2*.log

The main impact is this line of code. Why is this?My complete main function is at the bottom

ros::init(argc, argv, "lio_sam");    

The sentence that reported the error is the following sentence.

auto res = opt.optimize();

Motivation

Pitch

Alternatives

Additional context

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam");    

    gtsam::NonlinearFactorGraph graph;
    const gtsam::Vector3 para(1.0, 2.0, 1.0); // a,b,c的真实值
    double w_sigma = 1.0;                     // 噪声Sigma值
    cv::RNG rng; // OpenCV随机数产生器

    std::vector<double> x_data, y_data;

    for (int i = 0; i < 100; ++i)
    {
        double xi = i / 100.0;
        double yi = funct(para, xi) + rng.gaussian(w_sigma * w_sigma); // 加入了噪声数据
        auto noiseM = gtsam::noiseModel::Diagonal::Sigmas(Vector1(w_sigma)); // 噪声的维度需要与观测值维度保持一致
        graph.emplace_shared<curvfitFactor>(X(0), noiseM,xi, yi);     // 加入一元因子

        x_data.push_back(xi);
        y_data.push_back(yi);
    }

    gtsam::Values intial;
    intial.insert<gtsam::Vector3>(X(0), gtsam::Vector3(2.0, -1.0, 5.0));
    gtsam::GaussNewtonOptimizer opt(graph, intial); // 使用高斯牛顿优化
    std::cout << "initial error=" << graph.error(intial) << std::endl;
    auto res = opt.optimize();
    res.print("final res:");

    std::cout << "final error=" << graph.error(res) << std::endl;

    gtsam::Vector3 matX0 = res.at<gtsam::Vector3>(X(0));
    std::cout << "a b c: " << matX0 << "\n";

    int n = 5000;
    std::vector<double> x(n), y(n), w(n, 2);
    for (int i = 0; i < n; ++i)
    {
        x.at(i) = i * 1.0 / n;
        y.at(i) = exp(matX0(0) * x[i] * x[i] + matX0(1) * x[i] + matX0(2));
    }
    return 0;
}