map dir

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -578,16 +578,16 @@ void AdjustPPK::Compress(const vector<PCDPathInfo> &pcdPathVec,
         *pointcloud += pc;
     }
     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);
     dispatch->configOffset();

libs/voxel_map/libs/dispatch/dispatch.cpp

@@ -62,7 +62,7 @@ void Dispatch::configOffset()
         point.y -= offset_.y();
 //        point.z -= offset_.z();
     }
-
+    LOG(INFO) << "offset_: " << offset_.transpose();
 }
 
 void Dispatch::dispatch()
@@ -128,8 +128,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){