test

apps/CMakeLists.txt

@@ -9,19 +9,19 @@ include_directories(${PROJECT_SOURCE_DIR}/libs/locate_system)
 ###############################################
 # test
 
-#add_executable(localize_test localize_test.cpp)
+add_executable(localize_test localize_test.cpp)
 
-#target_include_directories(localize_test PUBLIC
-#    ${BOOST_INCLUDE_DIRS}
-#    ${PCL_INCLUDE_DIRS}
-#    localize
-#    )
+target_include_directories(localize_test PUBLIC
+    ${BOOST_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    localize
+    )
 
-#target_link_libraries(localize_test PUBLIC
-#    ${Boost_LIBRARIES}
-#    ${PCL_LIBRARIES}
-#    localize
-#    )
+target_link_libraries(localize_test PUBLIC
+    ${Boost_LIBRARIES}
+    ${PCL_LIBRARIES}
+    localize
+    )
 
 #add_executable(register_test register_test.cpp)
 

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/juefx/Documents/data/chengdu/19_6/voxels/"; \
+    std::string base_dir = "/home/wdw/Downloads/329546503.stream"; \
     PointCloudJ::Ptr raw_scan(new PointCloudJ);                      \
     PointCloudJ::Ptr scan(new PointCloudJ);                          \
     PointCloudJ::Ptr result(new PointCloudJ);                        \
-    pcl::io::loadPCDFile(base_dir + "result/basePcd/1676364900.039363.pcd", *raw_scan);            \
+    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd/1683806268.156870.pcd", *raw_scan);            \
     Eigen::Isometry3d mapPose;   \
-    mapPose.translation() = Eigen::Vector3d(12.0516637586524, 046.714495200166, 0-2.430956513463);  \
-    Eigen::Vector3d mapPoseRpy(0.00804236505308683, 00.0116266246183209, 0001.91314385703286);   \
+    mapPose.translation() = Eigen::Vector3d( 82.0294, 0370.65, 030.281);  \
+    Eigen::Vector3d mapPoseRpy(0.00306673, 00.0165335, 0002.68098);   \
     mapPose.linear() = Eigen::Matrix3d(       \
         Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
         * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
@@ -56,7 +56,7 @@ using namespace std;
     GnssPoint blh_final_pose;                                        \
     float Movement = 0.139191344380379;                                        \
     float Rotation = 0.0263764802366495;                                        \
-    int Precision = 3;                                        \
+    int Precision = 1;                                        \
     Eigen::Isometry3f final_pose;
 
 void test_ceres_map_matcher()
@@ -128,7 +128,7 @@ void test_onemesh_voxel_map_matcher()
 
     juefx::VoxelMapMatcher vmap_matcher(vmap_option);
 //    vmap_matcher.SetMapPath("./final");
-    vmap_matcher.SetMapPath(base_dir + "stream");
+    vmap_matcher.SetMapPath(base_dir/* + "stream"*/);
     vmap_matcher.loadArea(mapPose.translation());
 //    vmap_matcher.IsMapLoaded(init_pose);
 //    while (!vmap_matcher.IsMapLoaded(init_pose))
@@ -139,6 +139,10 @@ void test_onemesh_voxel_map_matcher()
 //    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/pcd/mapCloud.pcd", mapCloud);
     GuessPose guess_pose;
     guess_pose.use_gnss = false;
 //    guess_pose.gnss_point = init_pose;
@@ -157,7 +161,7 @@ void test_onemesh_voxel_map_matcher()
     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);
+//    pcl::io::savePCDFileBinary(base_dir + "/result/2result.pcd", *result);
 
     rusage usage;
     getrusage(RUSAGE_SELF, &usage);

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -62,6 +62,9 @@ void AdjustPPK::ReadBag(const string &bagPath)
     {
         const rosbag::MessageInstance &m = *viewIterator;
 //        LOG_EVERY_N(INFO, 1) << setprecision(15) << "m.getTime(): " << m.getTime().toSec();
+        if(m.getTime().toSec() < 1683806268.1){
+            continue;
+        }
         if(!m.isType<pandar_msgs::PandarScan>()){
             continue;
         }
@@ -69,6 +72,7 @@ void AdjustPPK::ReadBag(const string &bagPath)
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
         }
+        exit(0);
     }
 }