localize_test.cpp 7.66 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
#include <iostream>
#include <sys/resource.h>

#include <glog/logging.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include "localize/voxel_map_matcher.h"
using namespace std;

#define MIN_MOVEMENT 0.6
#define MIN_ROTATION 0.003
#define prepare_result_cloud(scan, pose, result) \
    result->points.clear();                      \
    for (auto &p : scan->points)                 \
    {                                            \
        Eigen::Vector3f v(p.x, p.y, p.z);        \
        Eigen::Vector3f new_v = pose * v;        \
        PointJ new_p;                            \
        new_p.x = new_v.x();                     \
        new_p.y = new_v.y();                     \
        new_p.z = new_v.z();                     \
        new_p.intensity = p.intensity;           \
        result->points.push_back(new_p);         \
    }

#define init_map_matcher_data()                               \
    PointCloudJ::Ptr scan(new PointCloudJ);                   \
    PointCloudJ::Ptr result(new PointCloudJ);                 \
    pcl::io::loadPCDFile("/home/juefx/Documents/data/test_data/final/ring100.pcd", *scan);                 \
    GnssPoint init_pose;                                      \
    init_pose.B = 31.2763319199 - 0.000002;                   \
    init_pose.L = 121.6587757024 - 0.000002;                  \
    init_pose.H = 16.424827 + 0.05;                           \
                                                              \
    Eigen::Vector3d gnss_rpy(0.765478, 1.597311, 249.582748); \
    gnss_rpy[1] = -gnss_rpy[1];                               \
    gnss_rpy[2] = 90 - gnss_rpy[2];                           \
    init_pose.rpy = gnss_rpy * M_PI / 180;                    \
    GnssPoint blh_final_pose;                                 \
    Eigen::Isometry3f final_pose;

#define init_big_map_matcher_data()                                  \
wangdawei's avatar
wangdawei committed
44
    std::string base_dir = "/home/wdw/Downloads/329546499/329546499.stream"; \
wangdawei's avatar
wangdawei committed
45 46 47
    PointCloudJ::Ptr raw_scan(new PointCloudJ);                      \
    PointCloudJ::Ptr scan(new PointCloudJ);                          \
    PointCloudJ::Ptr result(new PointCloudJ);                        \
wangdawei's avatar
wangdawei committed
48
    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd(3)/1683748706.956934_base.pcd", *raw_scan);            \
wangdawei's avatar
wangdawei committed
49
    Eigen::Isometry3d mapPose;   \
wangdawei's avatar
wangdawei committed
50 51
    mapPose.translation() = Eigen::Vector3d(-21.0299381284723, 0266.663172243435, 029.9718818825938);  \
    Eigen::Vector3d mapPoseRpy(0.0154573707048036, 0.0165835359033508, 002.47772534260029);   \
wangdawei's avatar
wangdawei committed
52 53 54 55 56 57 58
    mapPose.linear() = Eigen::Matrix3d(       \
        Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
        * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
        * Eigen::AngleAxisd(mapPoseRpy[0], Eigen::Vector3d::UnitX()));    \
    GnssPoint blh_final_pose;                                        \
    float Movement = 0.139191344380379;                                        \
    float Rotation = 0.0263764802366495;                                        \
wangdawei's avatar
wangdawei committed
59
    int Precision = 3;                                        \
wangdawei's avatar
wangdawei committed
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
    Eigen::Isometry3f final_pose;

void test_ceres_map_matcher()
{
    init_map_matcher_data();

    juefx::MapMatcherOption map_option;
    juefx::CeresVoxelMatcherOption vmap_option;
    map_option.filter_resolution = 0.5;
    map_option.cloud_range = 80;

    juefx::CeresVoxelMapMatcher vmap_matcher(map_option, vmap_option);
    vmap_matcher.SetMapPath(".");
//    vmap_matcher.SetMapPath("/home/juefx/localize_data/2220/");
    while (!vmap_matcher.IsMapLoaded(init_pose))
        sleep(1);
    vmap_matcher.MatchPointCloud(scan, init_pose, blh_final_pose, final_pose);
    prepare_result_cloud(scan, final_pose, result);
    pcl::io::savePCDFileBinary("2result_voxel.pcd", *result);
//    pcl::io::savePCDFileBinary("/home/juefx/localize_data/2220/2result.pcd", *result);
}

void test_fast_map_matcher()
{
    init_map_matcher_data();

    juefx::MapMatcherOption map_option;
    juefx::FastVoxelMatcherOption fmap_option;
    map_option.filter_resolution = 0.5;
    map_option.cloud_range = 80;

    juefx::FastVoxelMapMatcher fmap_matcher(map_option, fmap_option);
//    fmap_matcher.SetMapPath(".");
    fmap_matcher.SetMapPath("/home/juefx/Documents/data/test_data/stream");
    fmap_matcher.LoadMap();
//    fmap_matcher.IsMapLoaded(init_pose);
//    while (!fmap_matcher.IsMapLoaded(init_pose))
//        sleep(1);
//    LOG(INFO) << "scan->size(): " << scan->size();
//    fmap_matcher.MatchPointCloud(scan, init_pose, blh_final_pose, final_pose);
//    LOG(INFO) << 333;
//    prepare_result_cloud(scan, final_pose, result);
//    LOG(INFO) << 444;
//    pcl::io::savePCDFileBinary("2result_fast1.pcd", *result);

    GuessPose guess_pose;
    guess_pose.use_gnss = true;
    guess_pose.gnss_point = init_pose;
    guess_pose.movement = 1.2;
    guess_pose.rotation = 0.017;
    guess_pose.precision_type = MATCH_PRECISION_FAST;
    fmap_matcher.MatchPointCloud(scan, guess_pose, blh_final_pose, final_pose);
    prepare_result_cloud(scan, final_pose, result);
//    pcl::io::savePCDFileBinary("2result_fast2.pcd", *result);
    pcl::io::savePCDFileBinary("/home/juefx/Documents/data/test_data/2result.pcd", *result);
}

void test_onemesh_voxel_map_matcher()
{
    init_big_map_matcher_data();

    juefx::VoxelMapMatcherOption vmap_option;
wangdawei's avatar
wangdawei committed
122
    vmap_option.option.filter_resolution = 1.2;
wangdawei's avatar
wangdawei committed
123
    vmap_option.option.cloud_range = 80;
wangdawei's avatar
wangdawei committed
124
    vmap_option.fast_option.accepted_score = 0.35;
wangdawei's avatar
wangdawei committed
125 126 127 128 129 130
//    vmap_option.fast_option.Yaw_search_window *= vmap_option.fast_option.init_Yaw_scale;
//    vmap_option.fast_option.XY_search_window *= vmap_option.fast_option.init_XYZ_scale;
//    vmap_option.fast_option.Z_search_window *= vmap_option.fast_option.init_XYZ_scale;

    juefx::VoxelMapMatcher vmap_matcher(vmap_option);
//    vmap_matcher.SetMapPath("./final");
wangdawei's avatar
wangdawei committed
131
    vmap_matcher.SetMapPath(base_dir/* + "stream"*/);
wangdawei's avatar
wangdawei committed
132 133 134 135 136 137 138 139 140 141
    vmap_matcher.loadArea(mapPose.translation());
//    vmap_matcher.IsMapLoaded(init_pose);
//    while (!vmap_matcher.IsMapLoaded(init_pose))
//        sleep(1);

//    Eigen::Matrix4d map2base;
//    auto mapPoseInv = mapPose.inverse();
//    map2base.block(0, 3, 3, 1) = mapPoseInv.translation();
//    map2base.block(0, 0, 3, 3) = mapPoseInv.linear();
//    pcl::transformPointCloud(*raw_scan, *raw_scan, map2base.cast<float>());
wangdawei's avatar
wangdawei committed
142

wangdawei's avatar
wangdawei committed
143 144 145 146
//    Eigen::Matrix4f imu2base = Eigen::Matrix4f::Identity();
//    imu2base.block(0, 0, 3, 3) = Eigen::Matrix3f(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
//    pcl::transformPointCloud(*raw_scan, *raw_scan, imu2base);

wangdawei's avatar
wangdawei committed
147 148
    PointCloudJ mapCloud;
    pcl::transformPointCloud(*raw_scan, mapCloud, mapPose.cast<float>());
wangdawei's avatar
wangdawei committed
149
    pcl::io::savePCDFileBinary("/home/wdw/Downloads/test/mapCloud.pcd", mapCloud);
wangdawei's avatar
wangdawei committed
150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
    GuessPose guess_pose;
    guess_pose.use_gnss = false;
//    guess_pose.gnss_point = init_pose;
    guess_pose.map_point = mapPose;
    guess_pose.movement = Movement;
    guess_pose.rotation = Rotation;
    guess_pose.precision_type = Precision;
    if(guess_pose.movement < MIN_MOVEMENT){
        guess_pose.movement = MIN_MOVEMENT;
    }
    if(guess_pose.rotation < MIN_ROTATION){
        guess_pose.rotation = MIN_ROTATION;
    }
//    guess_pose.movement = 2;
//    guess_pose.rotation = 0.2;
    vmap_matcher.MatchPointCloud(raw_scan, guess_pose, blh_final_pose, final_pose);
    prepare_result_cloud(raw_scan, final_pose, result);
//    pcl::io::savePCDFileBinary("result_voxel.pcd", *result);
wangdawei's avatar
wangdawei committed
168
//    pcl::io::savePCDFileBinary(base_dir + "/result/2result.pcd", *result);
wangdawei's avatar
wangdawei committed
169 170 171 172 173 174 175 176 177 178 179 180 181 182

    rusage usage;
    getrusage(RUSAGE_SELF, &usage);
    LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
}

int main(int argc, char *argv[])
{
    //test_ceres_map_matcher();
//    test_fast_map_matcher();
    test_onemesh_voxel_map_matcher();

    return 0;
}