map dir

libs/voxel_map/libs/dispatch/dispatch.cpp

@@ -44,15 +44,28 @@ 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){
+            avgPosiVec.push_back(totalPosi / cnt);
+            totalPosi = Vector3d::Zero();
+            cnt = 0;
+        }
+        const auto &point = pointcloud_->at(i);
+        totalPosi.x() += point.x;
+        totalPosi.y() += point.y;
 //        z += point.z;
+        cnt++;
+    }
+    if(cnt > 0){
+        avgPosiVec.push_back(totalPosi / cnt);
+    }
+    for(const auto &avg : avgPosiVec){
+        offset_ += avg;
     }
-    offset_ = {x / pointcloud_->size(),
-               y / pointcloud_->size(),
-               z / pointcloud_->size()};
+    offset_ = offset_ / avgPosiVec.size();
     offset_ = {lround(offset_.x() / GridResolutionHigh) * GridResolutionHigh,
                lround(offset_.y() / GridResolutionHigh) * GridResolutionHigh,
                0