lili-om/LiLi-OM/include/factors/PriorFactor.h

#pragma once
#include <ros/assert.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include "utils/math_tools.h"

struct SpeedBiasPriorFactorAutoDiff {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    SpeedBiasPriorFactorAutoDiff(vector<double> speedBias) : speedBias_(speedBias) {}

    template <typename T> bool operator()(const T *speedBias, T *residuals) const {
        residuals[0] = T(15) * (speedBias[0] - T(speedBias_[0]));
        residuals[1] = T(15) * (speedBias[1] - T(speedBias_[1]));
        residuals[2] = T(15) * (speedBias[2] - T(speedBias_[2]));
        residuals[3] = T(15) * (speedBias[3] - T(speedBias_[3]));
        residuals[4] = T(15) * (speedBias[4] - T(speedBias_[4]));
        residuals[5] = T(15) * (speedBias[5] - T(speedBias_[5]));
        residuals[6] = T(15) * (speedBias[6] - T(speedBias_[6]));
        residuals[7] = T(15) * (speedBias[7] - T(speedBias_[7]));
        residuals[8] = T(15) * (speedBias[8] - T(speedBias_[8]));

        return true;
    }

    static ceres::CostFunction *Create(vector<double> speedBias_) {
        return (new ceres::AutoDiffCostFunction<SpeedBiasPriorFactorAutoDiff, 9, 9>(new SpeedBiasPriorFactorAutoDiff(speedBias_)));
    }

private:
    vector<double> speedBias_;
};