@@ -449,29 +449,32 @@ struct MetricRelativePoseErrorCostFunction {
449449 const EigenMatrix6d sqrt_information_j_;
450450};
451451
452- class LinearCostFunction : public ceres ::CostFunction {
453- public:
454- explicit LinearCostFunction (const double s) : s_(s) {
455- set_num_residuals (1 );
456- mutable_parameter_block_sizes ()->push_back (1 );
457- }
452+ // A cost function that wraps another one and whiten its residuals with an
453+ // isotropic covariance, i.e. assuming that the variance is identical in and
454+ // independent between each dimension of the residual.
455+ template <class CostFunction , typename ... Args>
456+ class IsotropicNoiseCostFunctionWrapper {
457+ class LinearCostFunction : public ceres ::CostFunction {
458+ public:
459+ explicit LinearCostFunction (const double s) : s_(s) {
460+ set_num_residuals (1 );
461+ mutable_parameter_block_sizes ()->push_back (1 );
462+ }
458463
459- bool Evaluate (double const * const * parameters,
460- double * residuals,
461- double ** jacobians) const final {
462- *residuals = **parameters * s_;
463- if (jacobians && *jacobians) {
464- **jacobians = s_;
464+ bool Evaluate (double const * const * parameters,
465+ double * residuals,
466+ double ** jacobians) const final {
467+ *residuals = **parameters * s_;
468+ if (jacobians && *jacobians) {
469+ **jacobians = s_;
470+ }
471+ return true ;
465472 }
466- return true ;
467- }
468473
469- private:
470- const double s_;
471- };
474+ private:
475+ const double s_;
476+ };
472477
473- template <class CostFunction , typename ... Args>
474- class IsotropicNoiseCostFunctionWrapper {
475478 public:
476479 static ceres::CostFunction* Create (Args&&... args, const double stddev) {
477480 THROW_CHECK_GT (stddev, 0.0 );
0 commit comments