map_matcher_interface.h 1.46 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
#pragma once

#include "utils/gnss.h"
#include "utils/pcl_point_type.h"
#include "utils/rigid3.h"
#include "utils/vector_point_type.h"

namespace juefx {

class VoxelMap;

struct MapMatcherOption {
    float filter_resolution;
    float cloud_range;
};

class MapMatcherInterface
{
public:
    MapMatcherInterface(const MapMatcherOption &option) {
        map_ = NULL;
        options_ = option;
    }
    ~MapMatcherInterface();

    bool SetMapPath(std::string path_name);

    bool LoadMap();

    bool loadArea(const Eigen::Vector3d &pose);

    bool IsMapLoaded(const GnssPoint blh_pose);

    bool IsMapLoaded(const Eigen::Vector3d &pose);

    float MatchPointCloud(const PointCloudJ::Ptr pcloud, const GnssPoint init_pose,
                          GnssPoint &final_pose, Eigen::Isometry3f &local_pose) {
        GuessPose guess_pose;
        guess_pose.gnss_point = init_pose;
        guess_pose.movement = guess_pose.rotation = 0;
        guess_pose.precision_type = MATCH_PRECISION_INIT;
        return MatchPointCloud(pcloud, guess_pose, final_pose, local_pose);
    }

    float MatchPointCloud(const PointCloudJ::Ptr pcloud, const GuessPose guess_pose,
                          GnssPoint &final_pose, Eigen::Isometry3f &local_pose);

protected:
    virtual float InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                             const Rigid3d &predict_pose, Rigid3d &estimate_pose) = 0;

    VoxelMap *map_;
    MapMatcherOption options_;
};

}