gtsam error report : Assertion `SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed

74 Views Asked by At

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

My custom unary factor looks like this:


// 自定义类名 : 继承于一元因子类<优化变量的数据类型>
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)));
    }
};
0

There are 0 best solutions below