cost_function_voxel.h 2.82 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 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
#pragma once

#include <ceres/ceres.h>
#include "../carto/transform/rigid_transform.h"

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

template <typename T>
using Rigid3 = cartographer::transform::Rigid3<T>;

namespace juefx {

class VoxelCostFunction {
public:
    static ceres::CostFunction *CreateAutoDiffCostFunction(
        const VectorCloud &point_cloud, const InterpolatedVoxelMap &voxel_map,
        double space_weight, double intensity_weight) {
        return new ceres::AutoDiffCostFunction<
            VoxelCostFunction, ceres::DYNAMIC /* residuals */,
            3 /* translation variables */, 4 /* rotation variables */>(
            new VoxelCostFunction(point_cloud, voxel_map, space_weight, intensity_weight),
            point_cloud.size());
    }

    template <typename T>
    bool operator()(const T *const translation, const T *const rotation,
                    T *const residual) const
    {
#ifdef VOXEL_MAP_CERES_DEBUG
        printf("-------- T %f %f %f, R %f %f %f %f\n",
               Jet::GetValue(translation[0]), Jet::GetValue(translation[1]), Jet::GetValue(translation[2]),
               Jet::GetValue(rotation[0]), Jet::GetValue(rotation[1]), Jet::GetValue(rotation[2]), Jet::GetValue(rotation[3]));
        voxel_map_.ResetDebugCount();
#endif
        const Rigid3<T> transform(
            Eigen::Map<const Eigen::Matrix<T, 3, 1>>(translation),
            Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3]));
        return Evaluate(transform, residual);
    }

private:
    VoxelCostFunction(const VectorCloud &point_cloud,
                      const InterpolatedVoxelMap &voxel_map,
                      double space_weight,
                      double intensity_weight)
        : point_cloud_(point_cloud), voxel_map_(voxel_map), 
          space_weight_(space_weight), intensity_weight_(intensity_weight) {}

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

    template <typename T>
    bool Evaluate(const Rigid3<T> &transform, T *const residual) const
    {
        double space_scale = space_weight_ /
                             std::sqrt(static_cast<double>(point_cloud_.size()));
        for (size_t i = 0; i < point_cloud_.size(); ++i)
        {
            const Eigen::Matrix<T, 3, 1> world = transform * point_cloud_[i].cast<T>();
            T space_prob, intensity_prob;
            if (voxel_map_.GetProbability(world[0], world[1], world[2], space_prob, intensity_prob))
                residual[i] = space_scale * (1. - space_prob);
            else
                residual[i] = (T)space_scale;
        }
        return true;
    }

    const VectorCloud &point_cloud_;
    const InterpolatedVoxelMap &voxel_map_;
    const double space_weight_;
    const double intensity_weight_;
};

}