map dir

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -553,7 +553,7 @@ void AdjustPPK::Compress(const vector<PCDPathInfo> &pcdPathVec,
         pcl::io::loadPCDFile(pcdInfo.pcdPath, pc);
         if(pcdInfo.taskName.size()){
             LocalCartesian tempProj(pcdInfo.taskCenter.y(),
-                                    pcdInfo.taskCenter.z(),
+                                    pcdInfo.taskCenter.x(),
                                     pcdInfo.taskCenter.z());
             vector<Vector3d> blhVec;
             blhVec.reserve(pc.size());
@@ -574,20 +574,20 @@ void AdjustPPK::Compress(const vector<PCDPathInfo> &pcdPathVec,
                 p.z = point.z();
                 pc.push_back(p);
             }
-            *pointcloud += pc;
         }
+        *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);

libs/voxel_map/libs/dispatch/dispatch.cpp

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