test

apps/CMakeLists.txt

@@ -87,26 +87,26 @@ target_link_libraries(localize_test PUBLIC
 #    dispatch
 #    serialize
 #    glog
-#    gflag
+#    gflags
 #    )
 ###############################################
 # restore_feature_map
-#add_executable(restore_feature_map extract.cpp)
+add_executable(restore_feature_map extract.cpp)
 
-#target_include_directories(restore_feature_map PUBLIC
-#    ${PCL_INCLUDE_DIRS}
-#    stream_info
-#    )
+target_include_directories(restore_feature_map PUBLIC
+    ${PCL_INCLUDE_DIRS}
+    stream_info
+    )
 
-#target_link_libraries(restore_feature_map PUBLIC
-#    ${PCL_LIBRARIES}
-#    ${Boost_LIBRARIES}
-#    ${GLOG_LIBRARIES}
-#    ${GFLAGS_LIBRARIES}
-#    stream_info
-#    glog
-#    gflag
-#    )
+target_link_libraries(restore_feature_map PUBLIC
+    ${PCL_LIBRARIES}
+    ${Boost_LIBRARIES}
+    ${GLOG_LIBRARIES}
+    ${GFLAGS_LIBRARIES}
+    stream_info
+    glog
+    gflags
+    )
 ###############################################
 # stream_test
 #add_executable(stream_test stream_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/wdw/Downloads/329546503.stream"; \
+    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/1683806268.156870.pcd", *raw_scan);            \
+    pcl::io::loadPCDFile("/home/wdw/Downloads/1683806268.056868.pcd", *raw_scan);            \
     Eigen::Isometry3d mapPose;   \
-    mapPose.translation() = Eigen::Vector3d( 82.0294, 0370.65, 030.281);  \
-    Eigen::Vector3d mapPoseRpy(0.00306673, 00.0165335, 0002.68098);   \
+    mapPose.translation() = Eigen::Vector3d(64.7009, 372.594, 29.5076);  \
+    Eigen::Vector3d mapPoseRpy(0.00618679, 0.00999828, 00002.4279);   \
     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 = 1;                                        \
+    int Precision = 3;                                        \
     Eigen::Isometry3f final_pose;
 
 void test_ceres_map_matcher()

libs/voxel_map/libs/area/maparea.h

@@ -197,44 +197,44 @@ public:
         }
         auto ret = currMesh_->GetCellValue(type, index, length);
 
-//        std::ofstream ofs;
-//        ofs.open("/home/juefx/grid_use_" + to_string(type) + ".txt", std::ios::app);
-//        int32_t blockIndexX = index.x.blockId;
-//        int32_t blockIndexY = index.y.blockId;
-//        uint32_t gridIndexX = index.x.gridId;
-//        uint32_t gridIndexY = index.y.gridId;
-//        Vector2f gridCenter;
-//        gridCenter.x() = blockIndexX * 23.04 + (0.5 + gridIndexX) * 0.09;
-//        gridCenter.y() = blockIndexY * 23.04 + (0.5 + gridIndexY) * 0.09;
-////        if(fabs(gridCenter.x() - (-0.045)) < 0.01
-////                && fabs(gridCenter.y() - 9.315) < 0.01){
-////            LOG(INFO) << "blockIndexX: " << blockIndexX << " blockIndexY: " << blockIndexY
-////                      << " gridIndexX: " << gridIndexX << " gridIndexY: " << gridIndexY
-////                      << " type: " << (uint16_t)type << " length: " << (uint16_t)length;
-////        }
-
-//        if(ret == nullptr){
-////            std::ofstream nullOfs;
-////            nullOfs.open("/home/juefx/null_" + to_string(type) + ".txt", std::ios::app);
-////            nullOfs << gridCenter.x() << "," << gridCenter.y() << "," << 0 << std::endl;
-////            nullOfs.close();
-//            return nullptr;
+        std::ofstream ofs;
+        ofs.open("/home/wdw/grid_use_" + to_string(type) + ".txt", std::ios::app);
+        int32_t blockIndexX = index.x.blockId;
+        int32_t blockIndexY = index.y.blockId;
+        uint32_t gridIndexX = index.x.gridId;
+        uint32_t gridIndexY = index.y.gridId;
+        Vector2f gridCenter;
+        gridCenter.x() = blockIndexX * 23.04 + (0.5 + gridIndexX) * 0.09 - currMesh_->GetOffset().x();
+        gridCenter.y() = blockIndexY * 23.04 + (0.5 + gridIndexY) * 0.09 - currMesh_->GetOffset().y();
+//        if(fabs(gridCenter.x() - (-0.045)) < 0.01
+//                && fabs(gridCenter.y() - 9.315) < 0.01){
+//            LOG(INFO) << "blockIndexX: " << blockIndexX << " blockIndexY: " << blockIndexY
+//                      << " gridIndexX: " << gridIndexX << " gridIndexY: " << gridIndexY
+//                      << " type: " << (uint16_t)type << " length: " << (uint16_t)length;
 //        }
-//        for(int i = 0; i < length; i++){
-//            for(int32_t j = 0;
-//                j < (ret + i)->height / 3;
-//                j++){
-//                int32_t indexZ = j + (ret + i)->floor / 3;
-//                float point[4];
-//                point[0] = gridCenter.x();
-//                point[1] = gridCenter.y();
-//                point[2] = ((float)indexZ + 0.5) * 3 / 100;
-//                point[3] = (ret + i)->intensity;
-//                ofs << point[0] << "," << point[1] << ","
-//                                << point[2] << "," << point[3] << std::endl;
-//            }
-//        }
-//        ofs.close();
+
+        if(ret == nullptr){
+//            std::ofstream nullOfs;
+//            nullOfs.open("/home/juefx/null_" + to_string(type) + ".txt", std::ios::app);
+//            nullOfs << gridCenter.x() << "," << gridCenter.y() << "," << 0 << std::endl;
+//            nullOfs.close();
+            return nullptr;
+        }
+        for(int i = 0; i < length; i++){
+            for(int32_t j = 0;
+                j < (ret + i)->height / 3;
+                j++){
+                int32_t indexZ = j + (ret + i)->floor / 3;
+                float point[4];
+                point[0] = gridCenter.x();
+                point[1] = gridCenter.y();
+                point[2] = ((float)indexZ + 0.5) * 3 / 100;
+                point[3] = (ret + i)->intensity;
+                ofs << point[0] << "," << point[1] << ","
+                                << point[2] << "," << point[3] << std::endl;
+            }
+        }
+        ofs.close();
 
         return ret;
     }

libs/voxel_map/libs/area/mesharea.cpp

@@ -63,6 +63,7 @@ void MeshArea::init()
     offset_.x() = header.offset_x();
     offset_.y() = header.offset_y();
     offset_.z() = header.offset_z();
+    LOG(INFO) << header.mesh_id() << " offset_: " << offset_.transpose();
     meshLevel_ = header.mesh_level();
     meshCenter_.x() = header.latitude();
     meshCenter_.y() = header.longtitude();