#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() \ std::string base_dir = "/home/wdw/Downloads/329546499/329546499.stream"; \ PointCloudJ::Ptr raw_scan(new PointCloudJ); \ PointCloudJ::Ptr scan(new PointCloudJ); \ PointCloudJ::Ptr result(new PointCloudJ); \ pcl::io::loadPCDFile("/home/wdw/Downloads/pcd(3)/1683748706.956934_base.pcd", *raw_scan); \ Eigen::Isometry3d mapPose; \ mapPose.translation() = Eigen::Vector3d(-21.0299381284723, 0266.663172243435, 029.9718818825938); \ Eigen::Vector3d mapPoseRpy(0.0154573707048036, 0.0165835359033508, 002.47772534260029); \ 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; \ int Precision = 3; \ 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; vmap_option.option.filter_resolution = 1.2; vmap_option.option.cloud_range = 80; vmap_option.fast_option.accepted_score = 0.35; // 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"); vmap_matcher.SetMapPath(base_dir/* + "stream"*/); 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>()); // 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); PointCloudJ mapCloud; pcl::transformPointCloud(*raw_scan, mapCloud, mapPose.cast<float>()); pcl::io::savePCDFileBinary("/home/wdw/Downloads/test/mapCloud.pcd", mapCloud); 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); // pcl::io::savePCDFileBinary(base_dir + "/result/2result.pcd", *result); 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; }