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/test/1683806268.156870.pcd", *raw_scan);            \
+    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd(3)/1683748706.956934_base.pcd", *raw_scan);            \
     Eigen::Isometry3d mapPose;   \
-    mapPose.translation() = Eigen::Vector3d(64.2653, 0372.09, 29.5085);  \
-    Eigen::Vector3d mapPoseRpy(0.00638869, 00.0101079, 0002.42708);   \
+    mapPose.translation() = Eigen::Vector3d(-21.0299381284723, 0266.663172243435, 029.9718818825938);  \
+    Eigen::Vector3d mapPoseRpy(0.0154573707048036, 0.0165835359033508, 002.47772534260029);   \
     mapPose.linear() = Eigen::Matrix3d(       \
         Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
         * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
@@ -119,9 +119,9 @@ void test_onemesh_voxel_map_matcher()
     init_big_map_matcher_data();
 
     juefx::VoxelMapMatcherOption vmap_option;
-    vmap_option.option.filter_resolution = 0.5;
+    vmap_option.option.filter_resolution = 1.2;
     vmap_option.option.cloud_range = 80;
-    vmap_option.fast_option.accepted_score = 0.45;
+    vmap_option.fast_option.accepted_score = 0.35;
 //    vmap_option.fast_option.Yaw_search_window *= vmap_option.fast_option.init_Yaw_scale;
 //    vmap_option.fast_option.XY_search_window *= vmap_option.fast_option.init_XYZ_scale;
 //    vmap_option.fast_option.Z_search_window *= vmap_option.fast_option.init_XYZ_scale;

libs/localize/voxel_map/fast_voxel_matcher.cc

@@ -377,7 +377,7 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
                 }
 #ifdef VOXEL_MAP_FAST_DEBUG
                 printf("score candidate, scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
-                       candidate.scan->index, candidate.search_depth,
+                       candidate.scan->yaw_index, candidate.search_depth,
                        candidate.offset.x(), candidate.offset.y(), candidate_xy.offset.z(),
                        candidate_xy.score, candidate_xy.low_resolution_score);
 #endif
@@ -396,7 +396,7 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
                 }
 #ifdef VOXEL_MAP_FAST_DEBUG
                 printf("score candidate, scan %d, depth %d, offset %d %d %d, score %f/%f=%f, low_score %f\n",
-                    candidate.scan->index, candidate.search_depth,
+                    candidate.scan->yaw_index, candidate.search_depth,
                     candidate.offset.x(), candidate.offset.y(), candidate_xy.offset.z(),
                     above_ground_score, ground_score, candidate_xy.score,
                     candidate_xy.low_resolution_score);
@@ -411,9 +411,11 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
                 no_map_cnt++;
             }
         }
-//        LOG(INFO) << "search_depth: " << candidates->front().search_depth
-//                  << " grid_indexes: " << candidates->front().scan->grid_indexes.size()
-//                  << ", no_map_cnt: " << no_map_cnt;
+#ifdef VOXEL_MAP_FAST_DEBUG
+        LOG(INFO) << "search_depth: " << candidates->front().search_depth
+                  << " grid_indexes: " << candidates->front().scan->grid_indexes.size()
+                  << ", no_map_cnt: " << no_map_cnt;
+#endif
     }
     if (0 != no_map_cnt) {
         float multi = (float)candidates->front().scan->grid_indexes.size() /
@@ -460,7 +462,7 @@ Candidate FastVoxelMatcher::BranchAndBound(const VoxelMap &voxel_map,
             }
 #ifdef VOXEL_MAP_FAST_DEBUG
             printf("====>>====>> scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
-                   candidate.scan->index, candidate.search_depth,
+                   candidate.scan->yaw_index, candidate.search_depth,
                    candidate.offset.x(), candidate.offset.y(), candidate.offset.z(),
                    candidate.score, candidate.low_resolution_score);
 #endif
@@ -481,7 +483,7 @@ Candidate FastVoxelMatcher::BranchAndBound(const VoxelMap &voxel_map,
         }
 #ifdef VOXEL_MAP_FAST_DEBUG
         printf(">>>> scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
-               candidate.scan->index, candidate.search_depth,
+               candidate.scan->yaw_index, candidate.search_depth,
                candidate.offset.x(), candidate.offset.y(), candidate.offset.z(),
                candidate.score, candidate.low_resolution_score);
 #endif

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/convert.cc

@@ -826,7 +826,7 @@ void Convert::init(pandar_msgs::PandarPacket packet)
         lidarmotorspeed = MOTOR_SPEED_600; //changing the speed,give enough size
       }
       m_iMotorSpeed = lidarmotorspeed;
-      printf("init mode: workermode: %x,return mode: %x,speed: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed);
+      printf("init mode: workermode: %x,return mode: %x,speed: %d, m_iLaserNum: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed, m_iLaserNum);
       changeAngleSize();
       changeReturnBlockSize();
 

libs/voxel_map/libs/area/maparea.h

@@ -201,44 +201,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;
     }