上传mota的计算指标的逻辑

MOTA/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+
+
+project(mota)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fprofile-arcs -ftest-coverage -pthread")
+#SET(CMAKE_BUILD_TYPE "Debug")
+SET(CMAKE_BUILD_TYPE "Release")
+
+# set(CMAKE_POSITION_INDEPENDENT_CODE TRUE)
+
+# include_directories(./)
+# include_directories(./include)
+# include_directories(./src/)
+# include_directories(./src/DeInflate)
+# include_directories(./src/Endecryption)
+
+add_executable(test main.cpp)
+# target_link_libraries(test 
+#     crypto
+#     ssl
+#     z
+#     )
+
+# add_executable(test1 test1.cpp)
+# target_link_libraries(test1 
+#     crypto
+#     ssl
+#     z
+#     )
+# add_executable(test3 test3.cpp)
+# target_link_libraries(test3 
+#     crypto
+#     ssl
+#     z
+#     )    
+
+# add_executable(main 
+#     main.cpp
+#     src/DeInflate/DeInflate.cpp
+#     )
+# target_link_libraries(main 
+#     crypto
+#     ssl
+#     z
+#     )    
+
+# add_library(generateLicense SHARED
+#     src/DeInflate/DeInflate.cpp
+#     src/GenerateLicense.cpp
+#     src/Endecryption/EncryptWithRsa.cpp
+#     src/Endecryption/Endecrypt.cpp
+#     )
+# target_link_libraries(generateLicense
+#     crypto.a
+#     ssl.a
+#     /usr/local/lib/libz.a
+#     )
+# add_executable(generate 
+#     Generate.cpp
+#     # src/CheckLicense.cpp
+#     # src/Endecryption/DecryptWithRsa.cpp
+#     )
+# target_link_libraries(generate 
+#     # crypto
+#     # ssl
+#     # z
+#     generateLicense
+#     dl
+#     )        
+    

MOTA/main.cpp

+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <map>
+#include <cmath>
+
+using namespace std;
+
+struct BoundingBox {
+    float x;
+    float y;
+    float z;
+    float length;
+    float width;
+    float height;
+    float orientation;
+};
+
+struct Object {
+    int object_id;
+    int frame_id;
+    BoundingBox bbox;
+    float confidence;
+    int type;
+    string class_name;
+};
+
+// 读取CSV格式的文件
+vector<Object> read_csv_file(const string& filename) {
+    vector<Object> data;
+    ifstream file(filename);
+    string line;
+    while (getline(file, line)) {
+        if (line.empty()) {
+            continue;
+        }
+        stringstream ss(line);
+        string field;
+        vector<string> fields;
+        while (getline(ss, field, ',')) {
+            fields.push_back(field);
+        }
+        Object obj;
+        obj.object_id = stoi(fields[0]);
+        obj.frame_id = stoi(fields[1]);
+        obj.bbox.x = stof(fields[2]);
+        obj.bbox.y = stof(fields[3]);
+        obj.bbox.z = stof(fields[4]);
+        obj.bbox.length = stof(fields[5]);
+        obj.bbox.width = stof(fields[6]);
+        obj.bbox.height = stof(fields[7]);
+        obj.bbox.orientation = stof(fields[8]);
+        obj.confidence = stof(fields[9]);
+        obj.type = stoi(fields[10]);
+        obj.class_name = fields[11];
+        data.push_back(obj);
+    }
+    return data;
+}
+
+// 计算IoU
+float calculate_iou_3d(const BoundingBox& bbox1, const BoundingBox& bbox2) {
+    float dx = abs(bbox1.x - bbox2.x);
+    float dy = abs(bbox1.y - bbox2.y);
+    float dz = abs(bbox1.z - bbox2.z);
+    float delta_l = abs(bbox1.length - bbox2.length);
+    float delta_w = abs(bbox1.width - bbox2.width);
+    float delta_h = abs(bbox1.height - bbox2.height);
+    float delta_o = abs(bbox1.orientation - bbox2.orientation);
+    float delta_xyz = sqrt(dx * dx + dy * dy + dz * dz);
+    float delta_dim = sqrt(delta_l * delta_l + delta_w * delta_w + delta_h * delta_h);
+    float delta_orien = delta_o;
+    float iou_xyz = 1.0f - delta_xyz / (bbox1.length + bbox1.width + bbox1.height + bbox2.length + bbox2.width + bbox2.height);
+    float iou_dim = 1.0f - delta_dim / (bbox1.length + bbox1.width + bbox1.height + bbox2.length + bbox2.width + bbox2.height);
+    float iou_orien = 1.0f - delta_orien / M_PI;
+    float iou = iou_xyz * iou_dim * iou_orien;
+    return iou;
+}
+
+// 计算匹配分数
+map<int, map<int, map<int, float>>> calculate_match_score_3d(
+        const vector<Object>& tracking_data, const vector<Object>& gt_data, float iou_threshold) {
+    map<int, map<int, map<int, float>>> match_score;
+    for (const auto& obj1 : tracking_data) {
+        for (const auto& obj2 : gt_data) {
+            if (obj1.frame_id == obj2.frame_id) {
+                float iou = calculate_iou_3d(obj1.bbox, obj2.bbox);
+                if (iou > iou_threshold) {
+                    match_score[obj1.frame_id][obj1.object_id][obj2.object_id] = iou;
+                }
+            }
+        }
+    }
+    return match_score;
+}
+
+// 确定真实值和跟踪结果之间的匹配关系
+map<int, map<int, int>> get_matches(const map<int, map<int, map<int, float>>>& match_score) {
+    map<int, map<int, int>> matches;
+    for (const auto& frame_pair : match_score) {
+        matches[frame_pair.first];
+        map<int, int> used_objects;
+        map<int, int> used_gt_objects;
+        for (const auto& object_pair : frame_pair.second) {
+            int object_id = object_pair.first;
+            int max_gt_object_id = -1;
+            float max_iou = 0;
+            for (const auto& gt_object_pair : object_pair.second) {
+                int gt_object_id = gt_object_pair.first;
+                float iou = gt_object_pair.second;
+                if (iou > max_iou && used_gt_objects.count(gt_object_id) == 0) {
+                    max_iou = iou;
+                    max_gt_object_id = gt_object_id;
+                }
+            }
+            if (max_gt_object_id != -1) {
+                matches[frame_pair.first][object_id] = max_gt_object_id;
+                used_objects[object_id] = 1;
+                used_gt_objects[max_gt_object_id] = 1;
+            }
+        }
+        for (const auto& object_pair : frame_pair.second) {
+            int object_id = object_pair.first;
+            if (used_objects.count(object_id) == 0) {
+                matches[frame_pair.first][object_id] = -1;
+            }
+        }
+        for (const auto& gt_object_pair : frame_pair.second.begin()->second) {
+            int gt_object_id = gt_object_pair.first;
+            if (used_gt_objects.count(gt_object_id) == 0) {
+                matches[frame_pair.first][-1] = gt_object_id;
+            }
+        }
+    }
+    return matches;
+}
+
+// 计算MOTA指标
+float calculate_mota_3d(const vector<Object>& tracking_data, const vector<Object>& gt_data, float iou_threshold) {
+    auto match_score = calculate_match_score_3d(tracking_data, gt_data, iou_threshold);
+    auto matches = get_matches(match_score);
+    int num_misses = 0;
+    int num_false_positives = 0;
+    int num_switches = 0;
+    int num_objects = 0;
+    for (const auto& frame_pair : matches) {
+        for (const auto& object_pair : frame_pair.second) {
+            int object_id = object_pair.first;
+            int gt_object_id = object_pair.second;
+            if (object_id == -1) {
+                num_misses++;
+            } else if (gt_object_id == -1) {
+                num_false_positives++;
+            } else {
+                num_objects++;
+                if (match_score[frame_pair.first][object_id][gt_object_id] < iou_threshold) {
+                    num_switches++;
+                }
+            }
+        }
+    }
+    float mota = 1.0f - (float)(num_misses + num_false_positives + num_switches) / num_objects;
+    return mota;
+}
+
+int main(int argc, char** argv) 
+{
+    std::string tracking_csv = "tracking_data.csv";
+    std::string gt_csv = "gt_data.csv";
+    if( argc == 3 )
+    {
+        tracking_csv = argv[1];
+        gt_csv = argv[2];
+    }
+    // 读取跟踪结果和真值
+    vector<Object> tracking_data = read_csv_file(tracking_csv);
+    vector<Object> gt_data = read_csv_file(gt_csv);
+
+    // 计算MOTA指标
+    float iou_threshold = 0.5;
+    float mota = calculate_mota_3d(tracking_data, gt_data, iou_threshold);
+    cout << "MOTA = " << mota << endl;
+
+    return 0;
+}
\ No newline at end of file

launch/jfx_node_ros.launch

 <launch>
   <arg name="parampath"  default="$(find jfx_node_ros)/config/params.yaml"/>
-  <arg name="run_mode"  default="1"/>
+  <arg name="run_mode"  default="2"/>
   <arg name="originDataPath"  default="/media/sf_originData/data"/>
   <arg name="gps_load_file"  default="/media/sf_originData/102022041610.nav/102022041610_traj.txt"/>
   <arg name="outPutSaveDir"  default="/media/sf_originData/detect_data/"/>
-  <arg name="playDetectTxt"  default="/media/sf_originData/detect_data/detect_gps_7.txt"/>
+  <arg name="playDetectTxt"  default="/media/sf_originData/detect_data/detect_gps_31.csv"/>
   <arg name="pcdFileFolder"  default="/media/sf_pcd_image/mesh"/>
   <arg name="image0FileFolder"  default="/media/sf_pcd_image/102022041610/image0"/>
   <arg name="image1FileFolder"  default="/media/sf_pcd_image/102022041610/image1"/>
+  <arg name="save_data_folder"  default="/media/sf_originData/save_data"/>
+  <arg name="start_timestamp"  default="/1650066200187"/>
+  <arg name="finish_timestamp"  default="/1650066240187"/>
   <arg name="send_pcd"  default="1"/>
   <arg name="send_image"  default="1"/>
+  <arg name="play_save_gps_csv"  default="/media/sf_originData/save_data/20230515-154612/detec_gps.csv"/>
+  <arg name="play_save_detect_folder"  default="/media/sf_originData/save_data/20230515-154612/detect"/>
+  <arg name="play_save_pcd_folder"  default="/media/sf_originData/save_data/20230515-154612/pcd"/>
+  <arg name="play_save_image0_folder"  default="/media/sf_originData/save_data/20230515-154612/image0"/>
+  <arg name="play_save_image1_folder"  default="/media/sf_originData/save_data/20230515-154612/image1"/>
   <node pkg="jfx_node_ros" type="jfx_node_ros" name="jfx_node_ros" output="screen" >
     <param name="parampath" value="$(arg parampath)" />
     <param name="run_mode" value="$(arg run_mode)" />
@@ -20,7 +28,15 @@
     <param name="pcdFileFolder" value="$(arg pcdFileFolder)" />
     <param name="image0FileFolder" value="$(arg image0FileFolder)" />
     <param name="image1FileFolder" value="$(arg image1FileFolder)" />
+    <param name="save_data_folder" value="$(arg save_data_folder)" />
+    <param name="start_timestamp" value="$(arg start_timestamp)" />
+    <param name="finish_timestamp" value="$(arg finish_timestamp)" />
     <param name="send_pcd" value="$(arg send_pcd)" />
     <param name="send_image" value="$(arg send_image)" />
+    <param name="play_save_gps_csv" value="$(arg play_save_gps_csv)" />
+    <param name="play_save_detect_folder" value="$(arg play_save_detect_folder)" />
+    <param name="play_save_pcd_folder" value="$(arg play_save_pcd_folder)" />
+    <param name="play_save_image0_folder" value="$(arg play_save_image0_folder)" />
+    <param name="play_save_image1_folder" value="$(arg play_save_image1_folder)" />
   </node>
 </launch>

src/SendRosMsg.hpp

@@ -16,6 +16,8 @@ public:
 
     int Play(ros::NodeHandle& nh);
 
+    int SaveFiles(ros::NodeHandle& nh);
+
     ros::Publisher m_pubFusionRes;//发送融合后的结果
     ros::Publisher m_pubLocalization;//发送定位结果
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息

src/main.cpp

@@ -19,11 +19,13 @@ struct FileInfo
     std::string fileName;
     std::string filePath;
     std::string pcdFile;
+    int carNum;
 };
 
 int LoadImageList(std::ifstream& file,std::string path,std::vector<FileInfo>& lists);
 int FindImageInfoFromTimstamp(uint64_t timestamp,std::vector<FileInfo>& lists,std::string& outFile);
 int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector<std::string>& values,std::vector<FileInfo>& image0List,std::vector<FileInfo>& image1List);
+int GetCarNumFromFile(std::string& file);
 
 int main(int argc, char** argv) {
     ros::init(argc, argv, "jfx_node_ros");
@@ -73,6 +75,8 @@ int main(int argc, char** argv) {
                                     info.pcdFile = subDir + "/output/" + ts + ".pcd";
                                     double timestamp = std::stod(ts);
                                     info.timestamp = timestamp*1000;
+                                    std::string carFile = directory + "/" + subFile;
+                                    info.carNum = GetCarNumFromFile(carFile);
                                     filenames.push_back(info);
                                 }
                             }
@@ -123,6 +127,8 @@ int main(int argc, char** argv) {
         do
         {
             std::getline(gpsFile, line);
+            if(line.at(line.size() -1) == '\r')
+                line.erase(line.end()-1);            
             if(line.empty())
                 break;
             std::vector<std::string> values;         
@@ -202,6 +208,14 @@ int main(int argc, char** argv) {
 
         ros::spin();
     }
+    else if(run_mode == 2)
+    {
+        LoadFileSendRosMsg sendMsg = {};
+        int ret = sendMsg.SaveFiles(phpriv_nh);
+        if(ret != 0)
+            printf("finish error ret = %d",ret);
+        ros::spin();        
+    }
 
 
     return 0;
@@ -220,14 +234,14 @@ int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector
         frame = 1;
         last_timestamp = fileInfo.timestamp;
         perid_start = fileInfo.timestamp;
-        std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".txt";
+        std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".csv";
         file.open(save_file,std::ios::out);        
         printf("save perid = %d  begin , file = %s\n",perid,save_file.c_str());
         needPrint = 1;
     }
     else
     {
-        if(fileInfo.timestamp - last_timestamp < 200)
+        if(fileInfo.timestamp - last_timestamp < 500)
         {
             frame++;
         }
@@ -235,9 +249,10 @@ int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector
         {
             file.close();
             printf("save perid = %d  end frame = %d, perid time = %llu\n",perid, frame, last_timestamp - perid_start);
+            printf("delay time = %llu\n",fileInfo.timestamp - last_timestamp);
             perid++;
             frame = 1;
-            std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".txt";
+            std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".csv";
             file.open(save_file,std::ios::out);             
             printf("save perid = %d  begin , file = %s\n",perid,save_file.c_str());
             perid_start = fileInfo.timestamp;
@@ -256,6 +271,7 @@ int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector
     {
         lineData << "," << iter;
     }
+    lineData << "," << fileInfo.carNum;
     lineData << std::endl;
     std::string str = lineData.str();
     if(needPrint == 1)
@@ -326,3 +342,75 @@ int FindImageInfoFromTimstamp(uint64_t timestamp,std::vector<FileInfo>& lists,st
     else
         return -1;
 }
+int GetCarNumFromFile(std::string& file)
+{
+    std::ifstream detFile;
+    detFile.open(file, std::ios::in);
+    int num = 0;
+    do
+    {
+        std::string line;
+        std::getline(detFile, line);
+        if(line.empty())
+            break;
+        std::vector<std::string> _values;         
+        std::stringstream _linestream(line);
+        std::string _cell;
+        _values.clear();
+        while (std::getline(_linestream, _cell, ' '))
+        {
+            _values.push_back(_cell);
+        }
+        float score = std::stof(_values[7]);
+        if(score < 0.4)
+            continue;
+        num++;
+    }while(1);
+    detFile.close();
+    return num;
+}
+
+/*
+以下是一个C++程序，可以读取给定的CSV文件，并根据每行中的信息创建目录和拷贝文件。你需要将程序中的`csvFilePath`修改为你要读取的CSV文件路径。请注意，此程序假定文件名中没有空格或特殊字符。
+
+```c++
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <filesystem>
+
+int main() {
+    const std::string csvFilePath = "/path/to/csv/file.csv"; // 修改为你的CSV文件路径
+    std::ifstream csvFile(csvFilePath);
+    if (!csvFile.is_open()) {
+        std::cerr << "Failed to open file: " << csvFilePath << std::endl;
+        return 1;
+    }
+    std::string line;
+    while (std::getline(csvFile, line)) {
+        std::istringstream lineStream(line);
+        std::string name, path1, path2;
+        if (std::getline(lineStream, name, ',') &&
+            std::getline(lineStream, path1, ',') &&
+            std::getline(lineStream, path2, ',')) {
+            // 创建目录
+            std::filesystem::path dirPath = std::filesystem::path(".") / name;
+            std::filesystem::create_directory(dirPath);
+            std::filesystem::create_directory(dirPath / "file1");
+            std::filesystem::create_directory(dirPath / "file2");
+            // 拷贝文件
+            std::filesystem::copy_file(path1, dirPath / "file1" / std::filesystem::path(path1).filename(), std::filesystem::copy_options::overwrite_existing);
+            std::filesystem::copy_file(path2, dirPath / "file2" / std::filesystem::path(path2).filename(), std::filesystem::copy_options::overwrite_existing);
+        }
+    }
+    return 0;
+}
+```
+
+这个程序使用了C++17中的`std::filesystem`库，需要在编译时加上`-std=c++17 -lstdc++fs`选项。例如，使用`g++`编译可以使用以下命令：
+
+```bash
+g++ -std=c++17 -lstdc++fs your_program.cpp -o your_program
+```
+
+*/
\ No newline at end of file