map dir

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -11,12 +11,13 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
 {
     LOG(INFO) << "ieBaseDir_: " << ieBaseDir_;
     LOG(INFO) << "mapDir_: " << mapDir_;
+    baseTaskName_ = "2329";
     if(!boost::filesystem::exists(ieBaseDir_ + "/baseCloud/")){
         boost::filesystem::create_directory(ieBaseDir_ + "/baseCloud/");
     }else{
         system(string("rm " + ieBaseDir_ + "/baseCloud/*").c_str());
     }
-    ReadCenter();
+    center_ = ReadCenter(baseTaskName_);
     LoadPPK();
     LoadMapInfo();
     CalLocalPose();
@@ -91,24 +92,26 @@ void AdjustPPK::ReadBag(const string &bagPath)
     }
 }
 
-void AdjustPPK::ReadCenter()
+Vector3d AdjustPPK::ReadCenter(const string& taskName)
 {
-    string centerPath = mapDir_ + "/2329/2329_station_jf.txt";
+    Vector3d center;
+    string centerPath = mapDir_ + taskName + "/" + taskName + "_station_jf.txt";
     std::ifstream ifs(centerPath);
     string line;
     vector<string> line_vec;
     if(ifs) {
         getline(ifs, line);
         boost::split(line_vec, line, boost::is_any_of(":"), boost::token_compress_on);
-        center_.x() = stof(line_vec.at(1));
+        center.x() = stof(line_vec.at(1));
         getline(ifs, line);
         boost::split(line_vec, line, boost::is_any_of(":"), boost::token_compress_on);
-        center_.y() = stof(line_vec.at(1));
+        center.y() = stof(line_vec.at(1));
         getline(ifs, line);
         boost::split(line_vec, line, boost::is_any_of(":"), boost::token_compress_on);
-        center_.z() = stof(line_vec.at(1));
+        center.z() = stof(line_vec.at(1));
     }
-    LOG(INFO) << "center_: " << center_.transpose();
+    LOG(INFO) << "center_: " << center.transpose();
+    return center;
 }
 
 void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
@@ -159,8 +162,8 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
 
     LocateCloud(cloudInfo);
 
-    PointCloudJ mathedCloud;
-    pcl::transformPointCloud(*cloudInfo.frame, mathedCloud, mapPose_.cast<float>());
+//    PointCloudJ mathedCloud;
+//    pcl::transformPointCloud(*cloudInfo.frame, mathedCloud, mapPose_.cast<float>());
 //    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + "_matched.pcd", mathedCloud);
 }
 
@@ -207,13 +210,15 @@ void AdjustPPK::LoadMapInfo()
 //            meshVec_.insert(make_pair(ulong(stoi(meshId)), meshMap));
 //        }
 //    }
+
     if(is_directory(mapDir_)){
-        directory_iterator mesh_iter(mapDir_);
-        directory_iterator mesh_end_iter;
-        for(; mesh_iter != mesh_end_iter; mesh_iter++) {
-            string meshDir = mesh_iter->path().string() + "/pcd_cart";
-            if(is_directory(meshDir)){
-                directory_iterator pcd_iter(meshDir);
+        directory_iterator task_iter(mapDir_);
+        directory_iterator task_end_iter;
+        for(; task_iter != task_end_iter; task_iter++) {
+            string taskName = task_iter->path().filename().string();
+            string pcdDir = task_iter->path().string() + "/pcd_cart";
+            if(is_directory(pcdDir)){
+                directory_iterator pcd_iter(pcdDir);
                 directory_iterator pcd_end_iter;
                 for(; pcd_iter != pcd_end_iter; pcd_iter++) {
                     if(pcd_iter->path().extension().string() != ".pcd"){
@@ -221,12 +226,18 @@ void AdjustPPK::LoadMapInfo()
                     }
                     auto fileName = pcd_iter->path().filename().string();
                     ulong meshId = stoi(split_string(fileName, '_').front());
+                    PCDPathInfo pcdPathInfo;
+                    pcdPathInfo.pcdPath = pcd_iter->path().string();
+                    if(baseTaskName_ != taskName){
+                        pcdPathInfo.taskName = taskName;
+                        pcdPathInfo.taskCenter = ReadCenter(taskName);
+                    }
                     if(meshVec_.find(meshId) == meshVec_.end()){
                         MashMap meshMap;
-                        meshMap.pcdPathVec.push_back(pcd_iter->path().string());
+                        meshMap.pcdPathVec.push_back(pcdPathInfo);
                         meshVec_.insert(make_pair(meshId, meshMap));
                     }else{
-                        meshVec_.at(meshId).pcdPathVec.push_back(pcd_iter->path().string());
+                        meshVec_.at(meshId).pcdPathVec.push_back(pcdPathInfo);
                     }
                 }
             }
@@ -529,13 +540,37 @@ void AdjustPPK::LoadMap()
     }
 }
 
-void AdjustPPK::Compress(const vector<string> &pcdPathVec,
+void AdjustPPK::Compress(const vector<PCDPathInfo> &pcdPathVec,
                          const string &streamPath)
 {
     PointCloudExport::Ptr pointcloud(new PointCloudExport);
-    for(const string &pcd_path : pcdPathVec){
+    for(const auto &pcdInfo : pcdPathVec){
         PointCloudExport pc;
-        pcl::io::loadPCDFile(pcd_path, pc);
+        pcl::io::loadPCDFile(pcdInfo.pcdPath, pc);
+        if(pcdInfo.taskName.size()){
+            LocalCartesian tempProj(pcdInfo.taskCenter.x(),
+                                    pcdInfo.taskCenter.y(),
+                                    pcdInfo.taskCenter.z());
+            vector<Vector3d> blhVec;
+            blhVec.reserve(pc.size());
+            for(const auto& p : pc){
+                Vector3d blh;
+                tempProj.Reverse(p.x, p.y, p.z,
+                                 blh.x(), blh.y(), blh.z());
+                blhVec.emplace_back(blh);
+            }
+            pc.clear();
+            for(const auto& blh : blhVec){
+                Vector3d point;
+                proj_.Forward(blh.x(), blh.y(), blh.z(),
+                              point.x(), point.y(), point.z());
+                PointExport p;
+                p.x = point.x();
+                p.y = point.y();
+                p.z = point.z();
+                pc.push_back(p);
+            }
+        }
         *pointcloud += pc;
     }
     boost::shared_ptr<Dispatch> dispatch(new Dispatch(pointcloud));

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -36,8 +36,14 @@ struct PPKPeriod{
 
 namespace juefx{
 
+struct PCDPathInfo{
+    string pcdPath;
+    string taskName = "";
+    Vector3d taskCenter;
+};
+
 struct MashMap{
-    vector<string> pcdPathVec;
+    vector<PCDPathInfo> pcdPathVec;
     boost::shared_ptr<VoxelMapMatcher> matcher;
 };
 
@@ -54,7 +60,7 @@ public:
     void ReadBag(const string &bagPath);
 
 private:
-    void ReadCenter();
+    Vector3d ReadCenter(const string &taskName);
 
     void OnReceivedCloud(const PPointCloud& cloud);
 
@@ -78,7 +84,7 @@ private:
 
     void LoadMap();
 
-    void Compress(const vector<string> &pcdPathVec,
+    void Compress(const vector<PCDPathInfo> &pcdPathVec,
                   const string &streamPath);
 
     bool LoadMesh(const string &streamPath,
@@ -121,6 +127,8 @@ private:
     std::ofstream ofs_;
 
     bool requestQuit_ = false;
+
+    string baseTaskName_;
 };
 
 } // end of namespace