cost_function_translation.h 1.68 KB
Newer Older
wangdawei's avatar
wangdawei committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
#pragma once

#include <ceres/ceres.h>

#include "../utils/vector_point_type.h"

namespace juefx {

class TranslationDeltaCostFunction {
public:
    static ceres::CostFunction *CreateAutoDiffCostFunction(
        const double scaling_factor, const double scaling_factor_z,
        const Eigen::Vector3d &target_translation) {
        return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunction,
                                               3 /* residuals */,
                                               3 /* translation variables */>(
            new TranslationDeltaCostFunction(scaling_factor,
                                             scaling_factor_z, target_translation));
    }

    template <typename T>
    bool operator()(const T *const translation, T *residual) const {
        residual[0] = scaling_factor_xy_ * (translation[0] - x_);
        residual[1] = scaling_factor_xy_ * (translation[1] - y_);
        residual[2] = scaling_factor_z_ * (translation[2] - z_);
        return true;
    }

private:
    explicit TranslationDeltaCostFunction(
        const double scaling_factor, const double scaling_factor_z,
        const Eigen::Vector3d &target_translation)
        : scaling_factor_xy_(scaling_factor),
          scaling_factor_z_(scaling_factor_z),
          x_(target_translation.x()),
          y_(target_translation.y()),
          z_(target_translation.z()) {}

    TranslationDeltaCostFunction(const TranslationDeltaCostFunction &) = delete;
    TranslationDeltaCostFunction &operator=(const TranslationDeltaCostFunction &) = delete;

    const double scaling_factor_xy_;
    const double scaling_factor_z_;
    const double x_;
    const double y_;
    const double z_;
};
}