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)));
    }
};