基于ROS、C++开发中使用的微分算法

使用前需要下载并导入opencv库，进行如下定义：

分别对应3维和2维的数组微分。

#include <map>
#include<std_msgs/Float64MultiArray.h>
#include<deque>
//#include<你的opencv位置>


struct PositionInfo{
  cv::Point3f position;
  cv::Point2f ax_ay;
  ros::Time timestamp;
};
// 定义用于存储上一帧中每个二维码的位置和时间戳的映射
std::map<int, cv::Point2f> last_positions;
std::map<int, ros::Time> last_timestamps;
std::map<int, cv::Point3f> last_positions_3d;
std::map<int, std::deque<PositionInfo>>position_history_map;

一、微分方法

下面介绍三种常见微分方法封装成的函数。

一.普通微分

优点：简单快捷。

缺点：误差较大。

void computeDif(int id, const cv::Point3f& position, const cv::Point2f& ax_ay, const ros::Time& time) {
    // 检查是否有前一个时间点的数据
    if (last_timestamps.find(id) != last_timestamps.end()) {
        // 计算时间间隔
        double time_interval = (time - last_timestamps[id]).toSec();

        if (time_interval > 0) {
            // 计算位置和角度的变化
            cv::Point3f pos_change = position - last_positions_3d[id];
            cv::Point2f ax_ay_change = ax_ay - last_ax_ay[id];

            // 计算速度
            speed_x = pos_change.x / time_interval;
            speed_y = pos_change.y / time_interval;
            speed_z = pos_change.z / time_interval;

            speed_ax = ax_ay_change.x / time_interval;
            speed_ay = ax_ay_change.y / time_interval;
        }
    }

    // 更新最后的位置、角度和时间戳
    last_positions_3d[id] = position;
    last_ax_ay[id] = ax_ay;
    last_timestamps[id] = time;
}

二、使用前三项的均值与后三项的均值微分

优点：误差在无噪声等外界干扰时会减小

缺点：受噪声影响大

void computeDif(int id,const cv::Point3f& position,const cv::Point2f& ax_ay,const ros::Time& time){
  auto& history = position_history_map[id];
  history.push_back({position,ax_ay,time});
  //history.push_back({ax_ay,time});

  if(history.size()>=6){
    cv::Point3f first_three_pos(0,0,0);
    cv::Point3f last_three_pos(0,0,0);
    cv::Point2f first_three_ax_ay(0,0);
    cv::Point2f last_three_ax_ay(0,0);

    for(int i = 0;i<3;++i){
      first_three_pos += history[i].position;
      last_three_pos += history[i+3].position;
      first_three_ax_ay += history[i].ax_ay;
      last_three_ax_ay += history[i+3].ax_ay;
    }

    first_three_pos *= (1.0/3);
    last_three_pos *= (1.0/3);
    first_three_ax_ay *= (1.0/3);
    last_three_ax_ay *= (1.0/3);

    cv::Point3f pos_change = last_three_pos-first_three_pos;
    cv::Point2f ax_ay_change = last_three_ax_ay - first_three_ax_ay;
    double time_interval = (history[5].timestamp - history[0].timestamp).toSec();

    if(time_interval >  0){
      speed_x = pos_change.x/time_interval;
      speed_y = pos_change.y/time_interval;
      speed_z = pos_change.z/time_interval;

      speed_ax = ax_ay_change.x/time_interval;
      speed_ay = ax_ay_change.y/time_interval;
    }

    history.pop_front();
  }
  last_positions_3d[id] = position;
  last_ax_ay[id] = ax_ay;
  last_timestamps[id] = time;
}

三、数值微分中的三项前向微分

void computeDif(int id, const cv::Point3f& position, const cv::Point2f& ax_ay, const ros::Time& time){
    auto& history = position_history_map[id];
    history.push_back({position, ax_ay, time});

    if (history.size() >= 3) {
        double h = (history[2].timestamp - history[1].timestamp).toSec();
        //double h2 = (history[1].timestamp - history[0].timestamp).toSec();

        if (h > 0 ) {
            
            cv::Point3f diff_pos = (-3 * history[0].position + 4 * history[1].position - history[2].position) / (2 * h);

           
            cv::Point2f diff_ax_ay = (-3 * history[0].ax_ay + 4 * history[1].ax_ay - history[2].ax_ay) / (2 * h);

            
            speed_x = diff_pos.x;
            speed_y = diff_pos.y;
            speed_z = diff_pos.z;
            speed_ax = diff_ax_ay.x;
            speed_ay = diff_ax_ay.y;
        }

        history.pop_front();
    }

    last_positions_3d[id] = position;
    last_ax_ay[id] = ax_ay;
    last_timestamps[id] = time;
}

属于比较高级的数值微分方法，使用这种方法可以抑制噪声，并且得到的结果误差很小。

如果结果依然不够理想，可以加入滤波，下面是一个一阶低通滤波的示例：

float alpha = 0.5; // 选择一个适当的alpha值，需要根据实际情况调整

// 为每个速度分量初始化滤波后的历史值
float filtered_speed_x = 0;
float filtered_speed_y = 0;
float filtered_speed_z = 0;

void lowPassFilter(float& filtered_speed,float new_speed){
  filtered_speed = alpha*new_speed+(1-alpha)*filtered_speed;
}

二.函数调用

在主函数的循环里进行调用：

          int id = tgts.targets[i].tracked_id;
          cv::Point3f current_position_3d(tgts.targets[i].px,tgts.targets[i].py,tgts.targets[i].pz);
          cv::Point2f current_ax_ay(tgts.targets[i].los_ax,tgts.targets[i].los_ay);
          ros::Time current_time = ros::Time::now();

          computeDif(id,current_position_3d,current_ax_ay,current_time);
          //滤波函数调用同理

---

总结

以上简单提供了三种微分方法的实现流程，可以根据自己需要进行修改。