map dir

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -578,16 +578,16 @@ void AdjustPPK::Compress(const vector<PCDPathInfo> &pcdPathVec,
         }
     }
     LOG(INFO) << "pointcloud->size() to dispatch: " << pointcloud->size();
-//    PointCloudExport downSampledPc;
-//    downSampledPc.reserve(pointcloud->size() * 0.06);
-//    for(size_t i = 0; i < pointcloud->size(); i++){
-//        if(i % 20 != 0){
-//            continue;
-//        }
-//        const auto& p = pointcloud->at(i);
-//        downSampledPc.push_back(p);
-//    }
-//    pcl::io::savePCDFileBinary(ieBaseDir_ + "/downSampledPc.pcd", downSampledPc);
+    PointCloudExport downSampledPc;
+    downSampledPc.reserve(pointcloud->size() * 0.06);
+    for(size_t i = 0; i < pointcloud->size(); i++){
+        if(i % 20 != 0){
+            continue;
+        }
+        const auto& p = pointcloud->at(i);
+        downSampledPc.push_back(p);
+    }
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/downSampledPc.pcd", downSampledPc);
 
     boost::shared_ptr<Dispatch> dispatch(new Dispatch(pointcloud));
     // dispatch.setMeshId(meshId);

libs/voxel_map/libs/dispatch/dispatch.cpp

@@ -44,15 +44,34 @@ boost::shared_ptr<GridLayer> Dispatch::getMeshLayer()
 
 void Dispatch::configOffset()
 {
-    double x = 0, y = 0, z = 0;
-    for(const auto &point : pointcloud_->points){
-        x += point.x;
-        y += point.y;
+    vector<Vector3d> avgPosiVec;
+    Vector3d totalPosi = Vector3d::Zero();
+    size_t cnt = 0;
+    for(size_t i = 0; i < pointcloud_->size(); i++){
+        if(i > 0 && i % 1000000 == 0){
+            LOG(INFO) << "totalPosi: " << totalPosi.transpose()
+                      << " cnt: " << cnt;
+            avgPosiVec.push_back(totalPosi / cnt);
+            totalPosi = Vector3d::Zero();
+            cnt = 0;
+        }
+        const auto &point = pointcloud_->at(i);
+        if(isnanf(point.x) || isnanf(point.y)){
+//            LOG_EVERY_N(INFO, 999999) << "nan";
+            continue;
+        }
+        totalPosi.x() += point.x;
+        totalPosi.y() += point.y;
 //        z += point.z;
+        cnt++;
+    }
+    if(cnt > 0){
+        avgPosiVec.push_back(totalPosi / cnt);
     }
-    offset_ = {x / pointcloud_->size(),
-               y / pointcloud_->size(),
-               z / pointcloud_->size()};
+    for(const auto &avg : avgPosiVec){
+        offset_ += avg;
+    }
+    offset_ = offset_ / avgPosiVec.size();
     offset_ = {lround(offset_.x() / GridResolutionHigh) * GridResolutionHigh,
                lround(offset_.y() / GridResolutionHigh) * GridResolutionHigh,
                0
@@ -128,8 +147,13 @@ void Dispatch::dispatchPoint(const PointExport &point)
 //    auto gi = bl[0]->getGridIndexByPoint(p);
 
     Vector2i blockIndex = meshLayer_->getGridIndexByPoint(point);
-    BlockGrid* block = reinterpret_cast<BlockGrid*>(
-                meshLayer_->getBlock(blockIndex).get());
+    BlockGrid* block;
+    try{
+        block = reinterpret_cast<BlockGrid*>(
+                    meshLayer_->getBlock(blockIndex).get());
+    } catch (exception e){
+        LOG(INFO) << blockIndex.transpose();
+    }
     vector<boost::shared_ptr<GridLayer>> blockLayers =
             block->getLayers();
     for(const auto &blockLayer : blockLayers){