test

apps/localize_test.cpp

@@ -41,14 +41,14 @@ using namespace std;
     Eigen::Isometry3f final_pose;
 
 #define init_big_map_matcher_data()                                  \
-    std::string base_dir = "/home/wdw/Downloads/329546499/329546499.stream"; \
+    std::string base_dir = "/home/wdw/Documents/0518/329546503.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);            \
+    pcl::io::loadPCDFile("/home/wdw/Documents/0518/1684349336.04388/1684349336.043877_mapped.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.translation() = Eigen::Vector3d(324.585, 764.021, -1.867);  \
+    Eigen::Vector3d mapPoseRpy( 0.00964, 0.00297, -1.59807);   \
     mapPose.linear() = Eigen::Matrix3d(       \
         Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
         * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
@@ -121,7 +121,8 @@ void test_onemesh_voxel_map_matcher()
     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.accepted_low_score = 0.5;
+    vmap_option.fast_option.accepted_score = 0.3;
 //    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;
@@ -130,23 +131,16 @@ void test_onemesh_voxel_map_matcher()
 //    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);
+    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>());
 
     PointCloudJ mapCloud;
     pcl::transformPointCloud(*raw_scan, mapCloud, mapPose.cast<float>());
-    pcl::io::savePCDFileBinary("/home/wdw/Downloads/test/mapCloud.pcd", mapCloud);
+//    pcl::io::savePCDFileBinary("/home/wdw/Downloads/test/mapCloud.pcd", mapCloud);
     GuessPose guess_pose;
     guess_pose.use_gnss = false;
 //    guess_pose.gnss_point = init_pose;
@@ -163,7 +157,7 @@ void test_onemesh_voxel_map_matcher()
 //    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);
+//    prepare_result_cloud(raw_scan, final_pose, result);
 //    pcl::io::savePCDFileBinary("result_voxel.pcd", *result);
 //    pcl::io::savePCDFileBinary(base_dir + "/result/2result.pcd", *result);
 

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -522,7 +522,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
 
     Quaterniond q(guess.linear());
     Vector3d rpy= RotationQuaternionToEulerVector(q);
-    LOG_EVERY_N(INFO, 100) << setprecision(15) << "cloud time: " << cloudInfo.timestamp
+    LOG(INFO) << setprecision(15) << "cloud time: " << cloudInfo.timestamp
               << " guessPose: " << guess.translation().transpose()
               << " rpy: " << rpy.transpose();
 //        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
@@ -684,7 +684,7 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
     option.option.filter_resolution = 1.0;
     option.option.cloud_range = 80;
     option.fast_option.accepted_score = 0.3;
-    option.fast_option.accepted_low_score = 0.4;
+    option.fast_option.accepted_low_score = 0.5;
     matcher.reset(new VoxelMapMatcher(option));
     matcher->SetMapPath(streamPath);
     return true;