voxel_map_matcher.cc

#include "voxel_map_matcher.h"

#include "carto/transform/transform.h"

#include "utils/timer.h"
#include "utils/util.hpp"
#include "utils/voxel_filter.h"
#include "voxel_map/voxel_map_interface.h"
#include "voxel_map/ceres_voxel_matcher.h"
#include "voxel_map/fast_voxel_matcher.h"

namespace juefx {

///////////////////////////////////////////////////////////
// method

inline float ceres_match(CeresVoxelMatcher *matcher, const VoxelMap *map,
                         const VectorCloud &vector_cloud, const Rigid3d &predict_pose, Rigid3d &estimate_pose) {
    int ceres_type = CERES_MODE_BASE;
#ifdef VOXEL_MAP_DEBUG
    ceres_type |= CERES_MODE_LOSS | CERES_MODE_DEBUG;
#endif
    const VectorCloud &low_cloud = vector_cloud;
    const VectorCloud &high_cloud = vector_cloud;
    ceres::Solver::Summary summary;
    Eigen::Vector4d loss = matcher->Match(
        predict_pose, high_cloud, low_cloud, *map, &estimate_pose, &summary, ceres_type);
    return loss[0];
}

inline float fast_match(FastVoxelMatcher *matcher, const VoxelMap *map, const FastVoxelMatcherOption& fast_options,
                        const VectorCloud &vector_cloud, const GuessPose guess_pose,
                        const Rigid3d &predict_pose, Rigid3d &estimate_pose) {
    int mode = FAST_MODE_GROUND;
    if (guess_pose.precision_type == MATCH_PRECISION_INIT)
        mode |= FAST_MODE_INIT;
    else if (guess_pose.precision_type == MATCH_PRECISION_FULL)
        mode |= FAST_MODE_GUESS;
    const auto param = matcher->GetMatchParam(*map, guess_pose.movement, guess_pose.rotation, mode);
#ifdef VOXEL_MAP_DEBUG
    printf("fast_match Search param: mode %d, start_depth %u, window xy %u / z %u / yaw %u, yaw step %f\n",
           param.mode, param.start_depth, param.xy_window_size, param.z_window_size,
           param.yaw_window_size, param.yaw_step);
#endif
    auto result = matcher->Match(predict_pose.cast<float>(), vector_cloud, *map, param);

    if (result != NULL) {
        estimate_pose = result->pose_estimate.cast<double>();
#ifdef VOXEL_MAP_DEBUG
        char buf[200];
        // add by weiji: Trans from estimate to predict
        // FIXME transform here is diffirent from transform in base frame
        Rigid3d delta_pose = predict_pose.inverse() * estimate_pose;
        auto delta_rpy = cartographer::transform::RotationQuaternionToEulerVector(delta_pose.rotation());
        snprintf(buf, 200, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }",
                    delta_pose.translation().x(), delta_pose.translation().y(), delta_pose.translation().z(),
                    delta_rpy.x(), delta_rpy.y(), delta_rpy.z());
        printf("fast_match return score: %f, low_score: %f, delta: %s\n",
                result->score, result->low_resolution_score, buf);
#endif
        return result->score;
    }
    return -1.f;
}

////////////////////////////////////////////////////////////
// Ceres voxel map matcher

CeresVoxelMapMatcher::CeresVoxelMapMatcher(const MapMatcherOption &option, const CeresVoxelMatcherOption ceres_options)
    : MapMatcherInterface(option) {
    ceres_options_ = ceres_options;
    matcher_ = new CeresVoxelMatcher(ceres_options);
}

CeresVoxelMapMatcher::~CeresVoxelMapMatcher() {
    delete matcher_;
}

float CeresVoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                                       const Rigid3d &predict_pose, Rigid3d &estimate_pose){
    return ceres_match(matcher_, map_, vector_cloud, predict_pose, estimate_pose);
}

////////////////////////////////////////////////////////////
// Fast voxel map matcher

FastVoxelMapMatcher::FastVoxelMapMatcher(const MapMatcherOption &option, const FastVoxelMatcherOption fast_options)
    : MapMatcherInterface(option)
{
    fast_options_ = fast_options;
    matcher_ = new FastVoxelMatcher(fast_options);
}

FastVoxelMapMatcher::~FastVoxelMapMatcher() {
    delete matcher_;
}

float FastVoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                                      const Rigid3d &predict_pose, Rigid3d &estimate_pose) {
    float score = fast_match(matcher_, map_, fast_options_, vector_cloud, guess_pose, predict_pose, estimate_pose);
    return score >= fast_options_.accepted_score? score: -1;
}

////////////////////////////////////////////////////////////
// Voxel map matcher

VoxelMapMatcher::VoxelMapMatcher(const VoxelMapMatcherOption &options) 
    : MapMatcherInterface(options.option) {
    ceres_options_ = options.ceres_option;
    fast_options_ = options.fast_option;
    ceres_matcher_ = new CeresVoxelMatcher(ceres_options_);
    fast_matcher_ = new FastVoxelMatcher(fast_options_);
}

VoxelMapMatcher::~VoxelMapMatcher() {
    delete ceres_matcher_;
    delete fast_matcher_;
}

float VoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                                  const Rigid3d &predict_pose, Rigid3d &estimate_pose) {
    float score = 0;
    Rigid3d result_pose;
    switch (guess_pose.precision_type)
    {
    case MATCH_PRECISION_FAST:
        score = fast_match(fast_matcher_, map_, fast_options_,
                           vector_cloud, guess_pose, predict_pose, estimate_pose);
        return (score >= fast_options_.accepted_score || score < 0) ? score : -score;

    case MATCH_PRECISION_HIGH:
        return ceres_match(ceres_matcher_, map_, vector_cloud, predict_pose, estimate_pose);

    case MATCH_PRECISION_FULL:
    case MATCH_PRECISION_INIT:
        score = fast_match(fast_matcher_, map_, fast_options_,
                           vector_cloud, guess_pose, predict_pose, estimate_pose);
        result_pose = score >= 0 ? estimate_pose : predict_pose; // -1 means failed
        return ceres_match(ceres_matcher_, map_, vector_cloud, result_pose, estimate_pose);

    default:
        return -1.f;
    }
}

}