test

apps/localize_test.cpp

@@ -45,10 +45,10 @@ using namespace std;
     PointCloudJ::Ptr raw_scan(new PointCloudJ);                      \
     PointCloudJ::Ptr scan(new PointCloudJ);                          \
     PointCloudJ::Ptr result(new PointCloudJ);                        \
-    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd/1683806268.056868.pcd", *raw_scan);            \
+    pcl::io::loadPCDFile("/home/wdw/Downloads/test/1683806268.156870.pcd", *raw_scan);            \
     Eigen::Isometry3d mapPose;   \
-    mapPose.translation() = Eigen::Vector3d(64.7009, 372.594, 29.5076);  \
-    Eigen::Vector3d mapPoseRpy(0.00618679, 0.00999828, 00002.4279);   \
+    mapPose.translation() = Eigen::Vector3d(64.2653, 0372.09, 29.5085);  \
+    Eigen::Vector3d mapPoseRpy(0.00638869, 00.0101079, 0002.42708);   \
     mapPose.linear() = Eigen::Matrix3d(       \
         Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
         * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
@@ -146,7 +146,7 @@ void test_onemesh_voxel_map_matcher()
 
     PointCloudJ mapCloud;
     pcl::transformPointCloud(*raw_scan, mapCloud, mapPose.cast<float>());
-    pcl::io::savePCDFileBinary("/home/wdw/Downloads/pcd/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;

libs/localize/voxel_map/voxel_map_interface.h

@@ -3,7 +3,7 @@
 //#include "voxel_map_stub.h"
 #include "voxel_map/interface/voxel_map.h"
 
-//#define VOXEL_MAP_DEBUG
-//#define VOXEL_MAP_FAST_DEBUG
+#define VOXEL_MAP_DEBUG
+#define VOXEL_MAP_FAST_DEBUG
 //#define VOXEL_MAP_CERES_DEBUG
 //#define VOXEL_MAP_CERES_OUTPUT_NUM 0

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -110,7 +110,7 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     if(cloudPacket.timestamp < 1683806268.156870){
         return;
     }
-    if(cloudPacket.timestamp > 1683806268.156870){
+    if(cloudPacket.timestamp > 1683806268.356870){
         exit(0);
     }
     pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + "_raw.pcd", cloud);
@@ -289,6 +289,7 @@ bool AdjustPPK::Undisort(
     PointCloudJ mappedFrame;
     CloudPreprocess::Instance()->Undisort(
                             frame, undisortedFrame, &mappedFrame, backPTime, &periodPose, framePose, increasePose);
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + "_mapped.pcd", mappedFrame);
     cloudInfo.frame = undisortedFrame;
     cloudInfo.filterPoseInfo.increasePose = increasePose;
     cloudInfo.timestamp = rawFrame.timestamp;
@@ -352,7 +353,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
         Vector3f rpy= RotationQuaternionToEulerVector(q);
         LOG(INFO) << "guessPose: " << mapPose.translation().transpose()
                   << " rpy: " << rpy.transpose();
-        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
+//        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
         GnssPoint finalGnssPose;
         GuessPose guessPose;
         guessPose.map_point = mapPose.cast<double>();

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/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;
+//        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;
+////        }
+
+//        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;
 //        }
-
-        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();
+//        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;
     }