map dir

libs/voxel_map/libs/dispatch/dispatch.cpp

@@ -48,7 +48,7 @@ void Dispatch::configOffset()
     Vector3d totalPosi = Vector3d::Zero();
     size_t cnt = 0;
     for(size_t i = 0; i < pointcloud_->size(); i++){
-        if(i > 0 && i % 500000 == 0){
+        if(i > 0 && i % 1000000 == 0){
             LOG(INFO) << "totalPosi: " << totalPosi.transpose()
                       << " cnt: " << cnt;
             avgPosiVec.push_back(totalPosi / cnt);
@@ -56,6 +56,10 @@ void Dispatch::configOffset()
             cnt = 0;
         }
         const auto &point = pointcloud_->at(i);
+        if(isnanf(point.x) || isnanf(point.y)){
+            LOG_EVERY_N(INFO, 9999) << "nan";
+            continue;
+        }
         totalPosi.x() += point.x;
         totalPosi.y() += point.y;
 //        z += point.z;