Assertion `SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed.

[zhao-zhibo]

Feature

I wrote a custom unary factor using gtsam. There is no problem compiling, but an error occurs when running. However, the error reported is strange. The line of code reporting the error is as follows:

GaussNewtonOptimizer opt(graph, intial);

The error is as follows：

lio_sam_curveFitting: /usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h:241: Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index) [with _Scalar = double; int _Rows = 1; int _Cols = 1; int _Options = 0; int _MaxRows = 1; int _MaxCols = 1; Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index = long int]: Assertion `SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed.
[lio_sam_curveFitting-2] process has died [pid 1903900, 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

Motivation

Pitch

Looking forward to finding the error

Alternatives

Additional context

my code is as follows:

class curvfitFactor : public gtsam::NoiseModelFactor1<gtsam::Vector3>
{
    double xi, yi; // 观测值

public:
    // 需要重写这个函数来定义误差和雅可比矩阵
    using NoiseModelFactor1<gtsam::Vector3>::evaluateError;

    // 因子的智能指针
    typedef std::shared_ptr<curvfitFactor> shared_ptr;

    curvfitFactor(gtsam::Key j, double x, double y, const gtsam::SharedNoiseModel &model)
        : gtsam::NoiseModelFactor1<gtsam::Vector3>(model, j), xi(x), yi(y)  {}

    ~curvfitFactor() override {}

    // 自定义因子一定要重写evaluateError函数(优化变量, 雅可比矩阵)
    Vector evaluateError(const gtsam::Vector3 &p, boost::optional<Matrix &> H = boost::none) const override
    {
        auto val = funct(p, xi);
        std::cout << "curvfitFactor 3:" << std::endl;
        if (H) // 残差为1维，优化变量为3维，雅可比矩阵为1*3
        {
            std::cout << "curvfitFactor 3.1:" << std::endl;
            gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 3);
            std::cout << "curvfitFactor 3.2:" << std::endl;
            Jac << xi * xi * val, xi * val, val;
            std::cout << "curvfitFactor 4:" << std::endl;
            (*H) = Jac;
        }
        std::cout << "curvfitFactor 6:" << std::endl;

        return gtsam::Vector1(val - yi); // 返回值为残差
    }

    gtsam::NonlinearFactor::shared_ptr clone() const override
    {
        return boost::static_pointer_cast<gtsam::NonlinearFactor>(
            gtsam::NonlinearFactor::shared_ptr(new curvfitFactor(*this)));
    }
};

[ProfFan]

Please look at the latest examples in the source, this code seems to be very ancient.

[zhao-zhibo]

I read the gtsam.pdf document and imitated the Defining Custom Factors section in Section 3.2. It ran without an error, but mine did. I simulated a hundred pairs of calculation observations for the exponential function y=exp(ax^2+bx+c)+w, where a, b, c are the variables to be estimated, and w is Gaussian noise, and then wrote this Unary factor.
I added a lot of logs in the evaluateError function to calculate the error term, but as you can see in the logs below, if(H) does not enter this function, I am not sure if there is something wrong here.
The error function I calculated is as follows

    Vector evaluateError(const gtsam::Vector3 &p, boost::optional<Matrix &> H = boost::none) const override
    {
        auto val = funct(p, xi);
        std::cout << "curvfitFactor 3:" << std::endl;
        if (H) // 残差为1维，优化变量为3维，雅可比矩阵为1*3
        {
            std::cout << "curvfitFactor 3.1:" << std::endl;
            gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 3);
            std::cout << "curvfitFactor 3.2:" << std::endl;
            Jac << xi * xi * val, xi * val, val;
            std::cout << "curvfitFactor 4:" << std::endl;
            (*H) = Jac;
        }
        std::cout << "curvfitFactor 6:" << std::endl;

        return gtsam::Vector1(val - yi); // 返回值为残差
    }

The generated log is as follows。
curvfitFactor 3: curvfitFactor 6: lio_sam_curveFitting: /usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h:241: Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index) [with _Scalar = double; int _Rows = 1; int _Cols = 1; int _Options = 0; int _MaxRows = 1; int _MaxCols = 1; Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index = long int]: Assertion 'SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed

[zhao-zhibo]

@ProfFan My problem was solved. It was a problem with the Eigen version. Changing the expression solved the problem.I asked a question on stackoverflow and someone else answered me.stackoverflow.
But I don't understand why this can solve the problem. And I don’t understand why this problem occurs?
Maybe gtsam needs to update the Eigen version to solve this problem.

[ProfFan]

You may have multiple Eigen versions on your system, which is explicitly disallowed by the Eigen authors

[zhao-zhibo]

@ProfFan Because I have Eigen installed on Ubuntu, and Eigen is included in gtsam. Do I need to directly delete the Eigen that gtsam depends on? Will this not affect the operation of gtsam?