120 lines
5.3 KiB
C++
120 lines
5.3 KiB
C++
|
|
/**
|
|
* @file Car.cc
|
|
* @author hong chu yuan (924273410@qq.com)
|
|
* @brief 车辆的实现文件
|
|
* @version 0.1
|
|
* @date 2023-03-30
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
|
|
#ifndef CAR_H
|
|
#define CAR_H
|
|
|
|
#include <vector>
|
|
#include <opencv2/core/core.hpp>
|
|
#include "Frame.h"
|
|
#include <vector>
|
|
#include <random>
|
|
#include <eigen3/Eigen/Core>
|
|
#include <eigen3/Eigen/Dense>
|
|
|
|
namespace ORB_SLAM2
|
|
{
|
|
class KeyFrame;
|
|
class Map;
|
|
class Frame;
|
|
|
|
class Car
|
|
{
|
|
public:
|
|
Car(Frame &CurrentFrame, vector<double> &bbox);
|
|
bool isBad(); // 查询这个车是否长时间未观测
|
|
|
|
/**
|
|
* @brief 更新速度和位置
|
|
* @param pose 目标中心的世界坐标
|
|
* @param delta_t 间隔时间
|
|
* @param vp_c 目标框顶点的像素坐标
|
|
* @param Rwc 相机的旋转
|
|
* @param twc 相机的平移
|
|
*
|
|
*/
|
|
void updateVelocityAndPose(cv::Mat &pose, double &delta_t, vector<cv::Point> &vp_c, cv::Mat &Rwc, cv::Mat &twc); // 更新车辆速度
|
|
bool isTracked();
|
|
bool isStatic();
|
|
|
|
void updateObservations(int CurrentFrameId); // 更新观测
|
|
void updateState(vector<double> &bbox, cv::Mat &Rwc, cv::Mat &twc, double &delta_t); // 更新速度及是否持续观测
|
|
|
|
/**
|
|
* @brief bev坐标转世界坐标
|
|
* @param vp_c 像素坐标
|
|
* @param Rwc 相机的旋转
|
|
* @param twc 相机的平移
|
|
* @param vmBboxWorldPose 世界坐标，输出值
|
|
*
|
|
*/
|
|
void bev2world(vector<cv::Point> &vp_c, vector<cv::Mat> &vmBboxWorldPose, cv::Mat &Rwc, cv::Mat &twc); // bev的目标框坐标转世界坐标
|
|
void bev2world(cv::Point &carBev, cv::Mat &CarWorldPose, cv::Mat &Rwc, cv::Mat &twc); // bev中心转世界坐标
|
|
void setTrackedState(bool state); // 设置状态
|
|
void update_untracked_Car(); // 更新 没有被当前目标框追踪到的目标位姿
|
|
void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts); // 获取目标框的四个点坐标
|
|
void updateTrackState(int ID); // 更新跟踪状态
|
|
void matrix_inv(cv::Mat T, cv::Mat T_inv); // 矩阵求逆
|
|
void updatetraj(); // 更新轨迹
|
|
void SetColor(); // 轨迹颜色
|
|
double get_Vel(double &t);
|
|
bool isErro();
|
|
void InCreaseFound(cv::Mat Tcw);
|
|
void world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw); // 世界坐标转bev像素
|
|
bool isInSight();
|
|
void kalmanfilter(Eigen::Vector2f &yk, double delta_t);
|
|
void Save_car_v();
|
|
|
|
public:
|
|
int mnCar_ID;
|
|
cv::Mat mmVelocity; // 车速
|
|
vector<cv::Mat> mvmBboxWorldPose; // 目标框
|
|
vector<cv::Mat> mvmBboxWorldPosePredict; // 预估的目标框位置
|
|
cv::Mat m_bev_K; // bev内参矩阵
|
|
cv::Mat m_bev_K_inv; // bev内参矩阵的逆
|
|
cv::Mat mRcl, mtcl; // 速度
|
|
vector<pair<int, int>> mv_pair_connect; // 观测关系：帧序号-bbox序号
|
|
cv::Mat mCarWorldPose; // 目标框世界坐标
|
|
int mvLostTrackTimes; // 是否持续丢失
|
|
cv::Mat mRbw; // 相机->bev的变换
|
|
double mdelta_w; // 角速度
|
|
double myaw; // 偏航角
|
|
bool mnTracked; // 是否被跟踪
|
|
vector<cv::Point3f> mtraj; // 轨迹
|
|
double mr; // 色彩
|
|
double mg; // 色彩
|
|
double mb; // 色彩
|
|
double mV; // m/s
|
|
int mvStatic;
|
|
cv::Mat Tow;
|
|
int mnVisual; // 观测次数 TODO 观测次数过少需要删除
|
|
bool mbErro; // 是否需要剔除
|
|
Eigen::Matrix<float, 2, 4> mkalman_H; // 卡尔曼滤波H矩阵
|
|
Eigen::Matrix2f mkalman_R = 0.8 * Eigen::Matrix2f::Identity(); // 卡尔曼滤波R矩阵
|
|
Eigen::Vector4f mmk_1;
|
|
Eigen::MatrixXf mPK_1 = Eigen::Matrix4f::Zero();
|
|
|
|
vector<cv::Point2f> mvpose; // 存储车辆每一时刻的位置信息
|
|
vector<cv::Point2f> mvvelocity; // 存储车辆每一时刻的位置信息
|
|
|
|
protected:
|
|
int mnLastFrameID; // 最后一次观测该车
|
|
int mnFirstFrameID; // 最初观测该车
|
|
bool mbBad; // 是否跟踪丢失
|
|
bool mbStatic; // 是否静态
|
|
int mnInsight; // 是否在视野内
|
|
bool mbInsight;
|
|
};
|
|
}
|
|
|
|
#endif // CAR_H
|