test

apps/localize_test.cpp

@@ -140,6 +140,10 @@ void test_onemesh_voxel_map_matcher()
 //    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/pcd/mapCloud.pcd", mapCloud);

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -69,6 +69,7 @@ void AdjustPPK::ReadBag(const string &bagPath)
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
         }
+        exit(0);
     }
 }
 

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -1230,6 +1230,7 @@ void Convert::calcPointXYZIT(const pandar_msgs::PandarPacket &packet, int cursor
         } else if (azimuthIdx < 0) {
           azimuthIdx += CIRCLE;
         }
+        LOG(INFO) << "azimuthIdx: " << azimuthIdx;
 
         point.x = xyDistance * m_fSinAllAngle[azimuthIdx];
         point.y = xyDistance * m_fCosAllAngle[azimuthIdx];

libs/voxel_map/libs/area/maparea.h

@@ -204,8 +204,8 @@ public:
         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();
+        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

libs/voxel_map/libs/area/mesharea.h

@@ -72,10 +72,8 @@ public:
             return false;
         }
         LOG(INFO) << "pose: " << pose.transpose();
-        double x = pose.x() - offset_.x();
-        double y = pose.y() - offset_.y();
         Vector2i shiftedIndexToLoad =
-                calShiftedIndex(x, y);
+                calShiftedIndex(pose.x(), pose.y());
         return checkBlockAtAreaToLoad(shiftedIndexToLoad);
     }
 
@@ -251,6 +249,8 @@ private:
 
     inline Vector2i calShiftedIndex(
             double x, double y) const{
+        x -= offset_.x();
+        y -= offset_.y();
         Vector2i blockIndex;
         blockIndex.x() = floor(x * 100 / blockResolution_);
         blockIndex.y() = floor(y * 100 / blockResolution_);