|
| 1 | +#pragma once |
| 2 | + |
| 3 | +#include <ceres/ceres.h> |
| 4 | + |
| 5 | +template <int Degree> |
| 6 | +struct PolynomialModel |
| 7 | +{ |
| 8 | + // Ceres requires number of params |
| 9 | + // to be known at compile time |
| 10 | + static constexpr int nparams = Degree + 1; |
| 11 | + |
| 12 | + template <typename T> |
| 13 | + static T eval(T x, const T* params) |
| 14 | + { |
| 15 | + const T p0 = params[0]; |
| 16 | + T result = p0; |
| 17 | + for (int i = 1; i < nparams; ++i) { |
| 18 | + const T p = params[i]; |
| 19 | + result += p * ceres::pow(x, T(i)); |
| 20 | + } |
| 21 | + |
| 22 | + return result; |
| 23 | + } |
| 24 | + // Not needed for least squares as ceres |
| 25 | + // supports boundaries |
| 26 | + template <typename T> |
| 27 | + static bool check_bounds(const T* params) |
| 28 | + { |
| 29 | + return true; |
| 30 | + } |
| 31 | +}; |
| 32 | + |
| 33 | +struct NoiseModel |
| 34 | +{ |
| 35 | + // Ceres requires number of params |
| 36 | + // to be known at compile time |
| 37 | + static constexpr int nparams = 3; |
| 38 | + |
| 39 | + template <typename T> |
| 40 | + static T eval(T f, const T* params) |
| 41 | + { |
| 42 | + const T fknee = params[0]; |
| 43 | + const T w = params[1]; |
| 44 | + const T alpha = params[2]; |
| 45 | + |
| 46 | + return w * (1.0 + ceres::pow(fknee / f, alpha)); |
| 47 | + } |
| 48 | + |
| 49 | + // Slightly hacky way of bounds checking but is |
| 50 | + // suggested by Ceres to ensure it never goes |
| 51 | + // out of bounds |
| 52 | + template <typename T> |
| 53 | + static bool check_bounds(const T* params) |
| 54 | + { |
| 55 | + const T w = params[1]; |
| 56 | + if (w <= 0.0) { |
| 57 | + return false; |
| 58 | + } |
| 59 | + return true; |
| 60 | + } |
| 61 | +}; |
| 62 | + |
| 63 | +// Model independent cost function for least-squares fitting |
| 64 | +template <typename Model> |
| 65 | +struct CostFunction |
| 66 | +{ |
| 67 | + using model = Model; |
| 68 | + |
| 69 | + CostFunction(int n, const double* x_data, const double* y_data) |
| 70 | + : n_pts(n), x(x_data), y(y_data) {} |
| 71 | + |
| 72 | + template <typename T> |
| 73 | + bool operator()(const T* const params, T* residual) const { |
| 74 | + for (int i = 0; i < n_pts; ++i) { |
| 75 | + T model = Model::eval(T(x[i]), params); |
| 76 | + residual[i] = T(y[i]) - model; |
| 77 | + } |
| 78 | + return true; |
| 79 | + } |
| 80 | + |
| 81 | + static ceres::Problem create(const int n, const double* xx, |
| 82 | + const double* yy, double* p) |
| 83 | + { |
| 84 | + ceres::Problem problem; |
| 85 | + |
| 86 | + problem.AddResidualBlock( |
| 87 | + new ceres::AutoDiffCostFunction<CostFunction<Model>, |
| 88 | + ceres::DYNAMIC, Model::nparams>( |
| 89 | + new CostFunction<Model>(n, xx, yy), n), nullptr, p); |
| 90 | + |
| 91 | + return problem; |
| 92 | + } |
| 93 | + |
| 94 | +private: |
| 95 | + const int n_pts; |
| 96 | + const double* x; |
| 97 | + const double* y; |
| 98 | +}; |
| 99 | + |
| 100 | +// Model independent Negative Log Likelihood for generalized |
| 101 | +// unconstrained minimization. This is to be used when data |
| 102 | +// has residuals that follow a chi^2(1) distribution. |
| 103 | +template <typename Model> |
| 104 | +struct NegLogLikelihood |
| 105 | +{ |
| 106 | + using model = Model; |
| 107 | + |
| 108 | + NegLogLikelihood(int n, const double* x_data, const double* y_data) |
| 109 | + : n_pts(n), x(x_data), y(y_data) {} |
| 110 | + |
| 111 | + template <typename T> |
| 112 | + bool operator()(const T* const params, T* cost) const |
| 113 | + { |
| 114 | + // Check bounds (saves a lot of time) |
| 115 | + if (!model::check_bounds(params)) { |
| 116 | + return false; |
| 117 | + } |
| 118 | + |
| 119 | + cost[0] = T(0.); |
| 120 | + for (int i = 0; i < n_pts; ++i) { |
| 121 | + T model = Model::eval(T(x[i]), params); |
| 122 | + cost[0] += ceres::log(model) + T(y[i]) / model; |
| 123 | + } |
| 124 | + |
| 125 | + return true; |
| 126 | + } |
| 127 | + |
| 128 | + static ceres::FirstOrderFunction* create(int n, const double* xx, |
| 129 | + const double* yy) |
| 130 | + { |
| 131 | + // Ceres takes ownership of pointers so no cleanup is required |
| 132 | + return new ceres::AutoDiffFirstOrderFunction<NegLogLikelihood<Model>, |
| 133 | + Model::nparams>(new NegLogLikelihood<Model>(n, xx, yy)); |
| 134 | + } |
| 135 | + |
| 136 | +private: |
| 137 | + const int n_pts; |
| 138 | + const double* x; |
| 139 | + const double* y; |
| 140 | +}; |
0 commit comments