上传测试修改

MOTA/CMakeLists.txt

@@ -4,18 +4,21 @@ 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_BUILD_TYPE "Debug")
+#SET(CMAKE_BUILD_TYPE "Release")
 
 # set(CMAKE_POSITION_INDEPENDENT_CODE TRUE)
 
-# include_directories(./)
+include_directories(/usr/include/eigen3)
 # include_directories(./include)
 # include_directories(./src/)
 # include_directories(./src/DeInflate)
 # include_directories(./src/Endecryption)
 
 add_executable(test main.cpp)
+
+add_executable(generate generate.cpp)
+add_executable(test1 test.cpp)
 # target_link_libraries(test 
 #     crypto
 #     ssl

MOTA/generate.cpp

+
+#include <string>
+#include <vector>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <memory>
+#include <map>
+#include <algorithm>
+#include <iomanip>
+#include <Eigen/Dense>
+
+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;
+    std::string class_name;
+    float v_x;
+    float v_y;
+};
+
+using ObjPtr = std::shared_ptr<Object>;
+
+// 读取CSV格式的文件
+int read_csv_file(const std::string& filename, std::vector<ObjPtr>& list,std::map<int,std::vector<ObjPtr>>& mapList)
+{
+    std::ifstream file(filename);
+    std::string line;
+    while (getline(file, line)) {
+        if (line.empty()) {
+            continue;
+        }
+        std::stringstream ss(line);
+        std::string field;
+        std::vector<std::string> fields;
+        while (getline(ss, field, ',')) {
+            fields.push_back(field);
+        }
+        if(fields[0] == "id")
+            continue;
+        ObjPtr obj = std::make_shared<Object>();
+        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];
+        obj->v_x = stof(fields[12]);
+        obj->v_y = stof(fields[13]);
+        list.push_back(obj);
+        if(mapList.find(obj->object_id) == mapList.end())
+        {
+            std::vector<ObjPtr> info;
+            mapList[obj->object_id] = info;
+        }
+        mapList[obj->object_id].push_back(obj);
+    }
+    file.close();
+    return 0;
+}
+
+// 二次函数 y = ax^2 + bx + c 的结构体
+struct QuadraticFunction {
+    double a, b, c;
+};
+struct Point
+{
+    double x;
+    double y;
+};
+// 拟合曲线并返回二次函数
+QuadraticFunction fitCurve(std::vector<Point>& points) {
+    // 计算 x 的平均值和 y 的平均值
+    double xMean = 0.0, yMean = 0.0;
+    for (size_t i = 0; i < points.size(); i++) {
+        xMean += points[i].x;
+        yMean += points[i].y;
+    }
+    xMean /= points.size();
+    yMean /= points.size();
+
+    // 计算二次函数系数
+    double a = 0.0, b = 0.0, c = 0.0;
+    for (size_t i = 0; i < points.size(); i++) {
+        double xDiff = points[i].x - xMean;
+        a += xDiff * xDiff;
+        b += xDiff;
+        c += xDiff * points[i].y;
+    }
+    b *= -2.0;
+    c *= -2.0;
+    a = (a * points.size() - b * b) / (a * points.size());
+    b = b / points.size();
+    c = c / points.size();
+
+    return {a, b, c};
+}
+
+// 拟合二次多项式
+Eigen::Vector3d fitQuadraticCurve(const std::vector<Point>& points) {
+    int n = points.size();
+    Eigen::MatrixXd A(n, 3);
+    Eigen::VectorXd b(n);
+
+    for (int i = 0; i < n; ++i) {
+        A(i, 0) = points[i].x * points[i].x;
+        A(i, 1) = points[i].x;
+        A(i, 2) = 1;
+
+        b(i) = points[i].y;
+    }
+
+    Eigen::Vector3d coeffs = A.colPivHouseholderQr().solve(b);
+    return coeffs;
+}
+
+// int main() {
+//     std::vector<double> x = {1.0, 2.0, 3.0, 4.0, 5.0};
+//     std::vector<double> y = {2.0, 7.0, 12.0, 17.0, 22.0};
+
+//     // 拟合曲线并输出二次函数系数
+//     QuadraticFunction curve = fitCurve(x, y);
+//     std::cout << "Fitted curve: y = " << curve.a << "x^2 + " << curve.b << "x + " << curve.c << std::endl;
+
+//     // 修改轨迹点在拟合曲线上的值
+//     for (size_t i = 0; i < x.size(); i++) {
+//         double newX = x[i];
+//         double newY = curve.a * newX * newX + curve.b * newX + curve.c;
+//         y[i] = newY;
+//     }
+
+//     // 输出修改后的轨迹点
+//     std::cout << "Modified trajectory points:" << std::endl;
+//     for (size_t i = 0; i < x.size(); i++) {
+//         std::cout << "(" << x[i] << ", " << y[i] << ")" << std::endl;
+//     }
+
+//     return 0;
+// }
+
+int main(int argc, char** argv) 
+{
+    std::string gt_csv = "gt_data.csv";
+    std::string gen_csv = "gt_gen.csv";
+    if( argc == 3 )
+    {
+        gt_csv = argv[1];
+        gen_csv = argv[2];
+    }
+    std::vector<ObjPtr> objList;
+
+    std::map<int,std::vector<ObjPtr>> objMaps;
+
+    read_csv_file(gt_csv,objList,objMaps);
+
+    //修正长宽高
+    for(auto iter: objMaps)
+    {
+        float totel_l = 0;
+        float totel_w = 0;
+        float totel_h = 0;
+        float totel_vx = 0;
+        float totel_vy = 0;
+        float totel_x = 0;
+        float totel_y = 0;
+        float totel_z = 0;
+        int v_num = 0;
+        float threhold = 1.0f;
+        for(auto its : iter.second)
+        {
+            totel_x += its->bbox.x;
+            totel_y += its->bbox.y;
+            totel_z += its->bbox.z;
+            totel_l += its->bbox.length;
+            totel_w += its->bbox.width;
+            totel_h += its->bbox.height;
+            totel_vx += its->v_x;
+            totel_vy += its->v_y;
+            if(its->v_x * its->v_x + its->v_y*its->v_y > threhold * threhold)
+                v_num++;
+        }
+        float x = totel_x/iter.second.size();
+        float y = totel_y/iter.second.size();
+        float z = totel_z/iter.second.size();
+        float length = totel_l/iter.second.size();
+        float width = totel_w/iter.second.size();
+        float heigth = totel_h/iter.second.size();
+        float v_x = totel_vx/iter.second.size();
+        float v_y = totel_vy/iter.second.size();
+        for(auto& its: iter.second)
+        {
+            its->bbox.length = length;
+            its->bbox.width = width;
+            its->bbox.height = heigth;
+            if(v_num < 5 && v_x * v_x + v_y*v_y < threhold * threhold) 
+            {
+                its->bbox.x = x;
+                its->bbox.y = y;
+                its->bbox.z = z;
+            }
+        }
+        if((v_x * v_x + v_y*v_y >= threhold * threhold || v_num >= 5) && iter.second.size() > 2)
+        {
+            std::vector<Point> points;
+            for(auto& its: iter.second)
+            {
+                Point pos = {its->bbox.x,its->bbox.y};
+                points.emplace_back(pos);
+            }
+            Eigen::Vector3d curve = fitQuadraticCurve(points);
+            // 修改轨迹点在拟合曲线上的值
+            for(auto& its: iter.second)
+            {
+                double newX = its->bbox.x;
+                double newY = curve(0) * newX * newX + curve(1) * newX + curve(2);
+                its->bbox.y = newY;
+            }            
+        }
+    }
+    std::ofstream saveFile;
+    saveFile.open(gen_csv,std::ios::out);
+
+    for(int i = 0; i < objList.size(); i++)
+    {
+        int id = objList[i]->object_id;
+        if(objMaps[id].size() <= 5)
+            continue;
+        std::stringstream lineData;
+        lineData << std::fixed << std::setprecision(8);
+        lineData << objList[i]->object_id << "," << objList[i]->frame_id << "," << objList[i]->bbox.x << "," << objList[i]->bbox.y << "," << objList[i]->bbox.z;
+        lineData << "," << objList[i]->bbox.length << "," << objList[i]->bbox.width << "," << objList[i]->bbox.height;
+        lineData << "," << objList[i]->bbox.orientation << "," <<  objList[i]->confidence << "," << objList[i]->type << "," << objList[i]->class_name;
+        lineData << std::endl;  
+        std::string str = lineData.str();
+        saveFile << str;
+    }
+  
+    
+    saveFile.close();
+    printf("finish \n");
+
+    return 0;
+}
\ No newline at end of file

MOTA/main.cpp

@@ -41,6 +41,8 @@ vector<Object> read_csv_file(const string& filename) {
         while (getline(ss, field, ',')) {
             fields.push_back(field);
         }
+        if(fields[0] == "id")
+            continue;
         Object obj;
         obj.object_id = stoi(fields[0]);
         obj.frame_id = stoi(fields[1]);
@@ -160,6 +162,7 @@ float calculate_mota_3d(const vector<Object>& tracking_data, const vector<Object
             }
         }
     }
+    printf("num_misses = %d, num_false_positives = %d, num_switches = %d, num_objects = %d\n",num_misses,num_false_positives,num_switches,num_objects);
     float mota = 1.0f - (float)(num_misses + num_false_positives + num_switches) / num_objects;
     return mota;
 }

MOTA/test.cpp

+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <Eigen/Dense>
+
+struct Point {
+    double x;
+    double y;
+};
+
+// 拟合二次多项式
+Eigen::Vector3d fitQuadraticCurve(const std::vector<Point>& points) {
+    int n = points.size();
+    Eigen::MatrixXd A(n, 3);
+    Eigen::VectorXd b(n);
+
+    for (int i = 0; i < n; ++i) {
+        A(i, 0) = points[i].x * points[i].x;
+        A(i, 1) = points[i].x;
+        A(i, 2) = 1;
+
+        b(i) = points[i].y;
+    }
+
+    Eigen::Vector3d coeffs = A.colPivHouseholderQr().solve(b);
+    return coeffs;
+}
+
+// 计算点到二次曲线的垂线交点
+Point computePerpendicularPoint(const Eigen::Vector3d& coeffs, const Point& point) {
+    double a = coeffs[0];
+    double b = coeffs[1];
+
+    double x = (2 * a * point.x - b) / (4 * a * a);
+    double y = a * x * x + b * x + coeffs[2];
+
+    return {x, y};
+}
+
+int main() {
+    // 示例轨迹点
+    std::vector<Point> points = {
+        {1, 1},
+        {2, 5},
+        {3, 8},
+        {4, 20},
+    };
+
+    // 拟合二次曲线
+    Eigen::Vector3d coeffs = fitQuadraticCurve(points);
+    std::cout << "二次曲线系数: " << coeffs.transpose() << std::endl;
+
+    // 更新轨迹点
+    for (Point& point : points) {
+        point = computePerpendicularPoint(coeffs, point);
+    }
+
+    // 输出更新后的轨迹点
+    std::cout << "更新后的轨迹点：" << std::endl;
+    for (const Point& point : points) {
+        std::cout << "(" << point.x << ", " << point.y << ")" << std::endl;
+    }
+
+    return 0;
+}
\ No newline at end of file

MOTA/test1.cpp

+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <cassert>
+
+// 多项式插值结构体
+struct Polynomial {
+    std::vector<double> a;  // 多项式系数
+};
+
+// 拟合曲线并返回多项式
+Polynomial fitCurve(const std::vector<double>& x, const std::vector<double>& y, int degree) {
+    assert(x.size() == y.size() && degree >= 1 && degree <= x.size() - 1);
+
+    // 构造矩阵 A 和向量 b
+    std::vector<std::vector<double>> A(x.size());
+    for (size_t i = 0; i < A.size(); i++) {
+        A[i].resize(degree + 1);
+        for (int j = 0; j <= degree; j++) {
+            A[i][j] = pow(x[i], j);
+        }
+    }
+
+    std::vector<double> b(y.begin(), y.end());
+
+    // 使用高斯消元法求解线性方程组
+    for (int k = 0; k <= degree; k++) {
+        for (int i = k + 1; i <= degree; i++) {
+            double factor = A[k][k] / A[i][k];
+            for (int j = k; j <= degree; j++) {
+                A[i][j] = A[k][j] - A[i][j] * factor;
+            }
+            b[i] = b[k] - b[i] * factor;
+        }
+    }
+
+    std::vector<double> a(degree + 1);
+
+    for (int i = degree; i >= 0; i--) {
+        double s = 0.0;
+        for (int j = i + 1; j <= degree; j++) {
+            s += A[i][j] * a[j];
+        }
+        a[i] = (b[i] - s) / A[i][i];
+    }
+
+    // 返回多项式
+    return {a};
+}
+
+// 计算多项式函数的值
+double evalPolynomial(const Polynomial& polynomial, double x) {
+    double y = 0.0;
+    for (int i = polynomial.a.size() - 1; i >= 0; i--) {
+        y = y * x + polynomial.a[i];
+    }
+    return y;
+}
+
+int main() {
+    std::vector<double> x = {1.0, 2.0, 3.0, 4.0, 5.0,6.0 ,7.0 ,8.0};
+    std::vector<double> y = {2.0, 5.0, 12.0, 17.0, 22.0,25,30,60};
+
+    // 拟合曲线并输出多项式
+    Polynomial curve = fitCurve(x, y, 2);
+    std::cout << "Fitted curve:" << std::endl;
+    for (double i = x.front(); i <= x.back(); i += 0.1) {
+        double j = evalPolynomial(curve, i);
+        std::cout << "(" << i << ", " << j << ")" << std::endl;
+    }
+
+    // 修改轨迹点在拟合曲线上的值
+    for (size_t i = 0; i < x.size(); i++) {
+        double newX = x[i];
+        double newY = evalPolynomial(curve, newX);
+        y[i] = newY;
+    }
+
+    // 输出修改后的轨迹点
+    std::cout << "Modified trajectory points:" << std::endl;
+    for (size_t i = 0; i < x.size(); i++) {
+        double newY = evalPolynomial(curve, x[i]);
+        std::cout << "(" << x[i] << ", " << newY << ")" << std::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="2"/>
+  <arg name="run_mode"  default="1"/>
   <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_31.csv"/>
+  <arg name="playDetectTxt"  default="/media/sf_originData/detect_data/detect_gps_3.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="start_timestamp"  default="/1650051001458"/>
+  <arg name="finish_timestamp"  default="/1650051031458"/>
   <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"/>
+  <arg name="play_save_gps_csv"  default="/media/sf_originData/save_data/60_90_csv31_1650064407084/detec_gps.csv"/>
+  <arg name="play_save_detect_folder"  default="/media/sf_originData/save_data/60_90_csv31_1650064407084/detect"/>
+  <arg name="play_save_pcd_folder"  default="/media/sf_originData/save_data/60_90_csv31_1650064407084/pcd"/>
+  <arg name="play_save_image0_folder"  default="/media/sf_originData/save_data/60_90_csv31_1650064407084/image0"/>
+  <arg name="play_save_image1_folder"  default="/media/sf_originData/save_data/60_90_csv31_1650064407084/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)" />