test

apps/localize_test.cpp

@@ -45,7 +45,7 @@ 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/1683806268.056868.pcd", *raw_scan);            \
+    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd/1683806268.056868.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);   \

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -1254,7 +1254,7 @@ void Convert::calcPointXYZIT(const pandar_msgs::PandarPacket &packet, int cursor
 //        LOG_EVERY_N(INFO, 256) << "azimuthIdx: " << azimuthIdx
 //                               << " m_fSinAllAngle[azimuthIdx]" << m_fSinAllAngle[azimuthIdx];
 
-        azimuthIdx = CIRCLE - azimuthIdx;
+//        azimuthIdx = CIRCLE - azimuthIdx;
         point.x = xyDistance * m_fSinAllAngle[azimuthIdx];
         point.y = xyDistance * m_fCosAllAngle[azimuthIdx];
         point.z = distance * m_fSinAllAngle[pitchIdx];

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;
-////        }
-
-//        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;
     }