480 lines
16 KiB
C++
480 lines
16 KiB
C++
|
||
#include "Car.h"
|
||
|
||
namespace ORB_SLAM2
|
||
{
|
||
Car::Car(Frame &CurrentFrame, vector<double> &bbox) : mnLastFrameID(CurrentFrame.mnId),
|
||
mnFirstFrameID(CurrentFrame.mnId),
|
||
mbBad(false),
|
||
mvLostTrackTimes(0),
|
||
mmVelocity(cv::Mat::eye(4, 4, CV_32FC1)),
|
||
mnVisual(1),
|
||
myaw(bbox[7]),
|
||
mdelta_w(0),
|
||
mbStatic(false),
|
||
mbErro(false),
|
||
mvStatic(0),
|
||
mV(0),
|
||
mnInsight(0),
|
||
Tow(cv::Mat::eye(4, 4, CV_32FC1)),
|
||
mbInsight(true)
|
||
{
|
||
static int instanceCount = 0; // 计数器
|
||
|
||
instanceCount++;
|
||
mnCar_ID = instanceCount;
|
||
mRcl = mmVelocity.rowRange(0, 3).colRange(0, 3);
|
||
mtcl = mmVelocity.rowRange(0, 3).col(3);
|
||
m_bev_K = (cv::Mat_<float>(3, 3) << 608.0 / 50.0, 0, 304, 0, 608.0 / 50.0, 608, 0, 0, 1);
|
||
m_bev_K_inv = (cv::Mat_<float>(3, 3) << 50.0 / 608.0, 0, -25, 0, 50.0 / 608.0, -50, 0, 0, 1);
|
||
mRbw = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // world->bev
|
||
vector<cv::Point> vp_c;
|
||
get_bbx(bbox, vp_c);
|
||
bev2world(vp_c, mvmBboxWorldPose, CurrentFrame.mRwc, CurrentFrame.mOw);
|
||
|
||
mvmBboxWorldPosePredict.resize(4);
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
mvmBboxWorldPosePredict[i] = mvmBboxWorldPose[i];
|
||
}
|
||
cv::Point carBev;
|
||
carBev.x = bbox[1];
|
||
carBev.y = bbox[2];
|
||
bev2world(carBev, mCarWorldPose, CurrentFrame.mRwc, CurrentFrame.mOw);
|
||
updatetraj();
|
||
SetColor();
|
||
mkalman_H << 1, 0, 0, 0,
|
||
0, 1, 0, 0;
|
||
}
|
||
|
||
bool Car::isStatic()
|
||
{
|
||
return mbStatic;
|
||
}
|
||
|
||
bool Car::isErro()
|
||
{
|
||
return mbErro;
|
||
}
|
||
|
||
double Car::get_Vel(double &t)
|
||
{
|
||
mV = sqrt(pow(mtcl.at<float>(0, 0), 2) + pow(mtcl.at<float>(1, 0), 2) + pow(mtcl.at<float>(2, 0), 2)) / 0.1;
|
||
if (mV * 3.6 > 120)
|
||
{
|
||
mbBad = true;
|
||
}
|
||
if (mV < 2)
|
||
{
|
||
if (mvLostTrackTimes == 0)
|
||
{
|
||
mvStatic++;
|
||
}
|
||
mmVelocity = cv::Mat::eye(4, 4, CV_32FC1);
|
||
mRcl = mmVelocity.rowRange(0, 3).colRange(0, 3);
|
||
mtcl = mmVelocity.rowRange(0, 3).col(3);
|
||
mV = 0;
|
||
}
|
||
else
|
||
{
|
||
mvStatic = 0;
|
||
mbStatic = false;
|
||
}
|
||
|
||
if (mvStatic > 6)
|
||
{
|
||
mbStatic = true;
|
||
}
|
||
return mV;
|
||
}
|
||
|
||
void Car::SetColor()
|
||
{
|
||
int min = 0, max = 255;
|
||
random_device seed; // 硬件生成随机数种子
|
||
ranlux48 engine(seed()); // 利用种子生成随机数引擎
|
||
uniform_int_distribution<> distrib(min, max); // 设置随机数范围，并为均匀分布
|
||
int random_r = distrib(engine); // 随机数
|
||
int random_g = distrib(engine); // 随机数
|
||
int random_b = distrib(engine); // 随机数
|
||
|
||
mr = random_r / 255.0;
|
||
mg = random_g / 255.0;
|
||
mb = random_b / 255.0;
|
||
}
|
||
|
||
void Car::get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts)
|
||
{
|
||
cv::Point p1, p2, p3, p4;
|
||
double x = label1[1];
|
||
double y = label1[2];
|
||
double w = label1[3];
|
||
double l = label1[4];
|
||
double yaw = label1[7];
|
||
double cos_yaw, sin_yaw;
|
||
cos_yaw = cos(yaw);
|
||
sin_yaw = sin(yaw);
|
||
// front left
|
||
p1.x = x - w / 2 * cos_yaw - l / 2 * sin_yaw;
|
||
p1.y = y - w / 2 * sin_yaw + l / 2 * cos_yaw;
|
||
|
||
// rear left
|
||
p2.x = x - w / 2 * cos_yaw + l / 2 * sin_yaw;
|
||
p2.y = y - w / 2 * sin_yaw - l / 2 * cos_yaw;
|
||
|
||
// rear right
|
||
p3.x = x + w / 2 * cos_yaw + l / 2 * sin_yaw;
|
||
p3.y = y + w / 2 * sin_yaw - l / 2 * cos_yaw;
|
||
|
||
// front right
|
||
p4.x = x + w / 2 * cos_yaw - l / 2 * sin_yaw;
|
||
p4.y = y + w / 2 * sin_yaw + l / 2 * cos_yaw;
|
||
|
||
pts.push_back(p1);
|
||
pts.push_back(p2);
|
||
pts.push_back(p3);
|
||
pts.push_back(p4);
|
||
// cout << pts << endl;
|
||
}
|
||
|
||
void Car::bev2world(vector<cv::Point> &vp_c, vector<cv::Mat> &vmBboxWorldPose, cv::Mat &Rwc, cv::Mat &twc)
|
||
{
|
||
// cout << vp_c << endl;
|
||
vmBboxWorldPose.clear();
|
||
for (auto p_c : vp_c)
|
||
{
|
||
cv::Mat p_uv_c_bev, p_cam_c_bev, p_cam_c_cam, p_cam_w_cam, R_ij;
|
||
p_uv_c_bev = cv::Mat::zeros(3, 1, CV_32FC1);
|
||
p_uv_c_bev.at<float>(0, 0) = p_c.x; // 取bev像素坐标
|
||
p_uv_c_bev.at<float>(1, 0) = p_c.y;
|
||
p_uv_c_bev.at<float>(2, 0) = 1.0;
|
||
p_cam_c_bev = m_bev_K_inv * p_uv_c_bev; // bev像素坐标转bev相机坐标
|
||
p_cam_c_cam = mRbw.t() * p_cam_c_bev; // bev相机坐标转cam的相机坐标
|
||
|
||
p_cam_c_cam.at<float>(1, 0) = -1.0;
|
||
p_cam_w_cam = Rwc * p_cam_c_cam + twc; // 当前cam相机坐标转到世界坐标
|
||
vmBboxWorldPose.push_back(p_cam_w_cam);
|
||
}
|
||
}
|
||
|
||
void Car::bev2world(cv::Point &carBev, cv::Mat &CarWorldPose, cv::Mat &Rwc, cv::Mat &twc)
|
||
{
|
||
cv::Mat p_uv_c_bev, p_cam_c_bev, p_cam_c_cam, p_cam_w_cam, R_ij;
|
||
p_uv_c_bev = cv::Mat::zeros(3, 1, CV_32FC1);
|
||
p_uv_c_bev.at<float>(0, 0) = carBev.x; // 取bev像素坐标
|
||
p_uv_c_bev.at<float>(1, 0) = carBev.y;
|
||
p_uv_c_bev.at<float>(2, 0) = 1.0;
|
||
p_cam_c_bev = m_bev_K_inv * p_uv_c_bev; // bev像素坐标转bev相机坐标
|
||
p_cam_c_cam = mRbw.t() * p_cam_c_bev; // bev相机坐标转cam的相机坐标
|
||
p_cam_c_cam.at<float>(1, 0) = -1.0;
|
||
CarWorldPose = Rwc * p_cam_c_cam + twc; // 当前cam相机坐标转到世界坐标
|
||
}
|
||
|
||
bool Car::isBad()
|
||
{
|
||
return mbBad;
|
||
}
|
||
|
||
bool Car::isInSight()
|
||
{
|
||
return mbInsight;
|
||
}
|
||
|
||
void Car::InCreaseFound(cv::Mat Tcw)
|
||
{
|
||
mbInsight = false;
|
||
|
||
cv::Mat p_3d_w = mCarWorldPose;
|
||
cv::Mat Rcw, tcw, R_ij;
|
||
cv::Mat p_3d_w_mat = cv::Mat::zeros(3, 1, CV_32FC1);
|
||
int u, v;
|
||
p_3d_w_mat.at<float>(0, 0) = p_3d_w.at<float>(0, 0); // 取bev像素坐标
|
||
p_3d_w_mat.at<float>(1, 0) = p_3d_w.at<float>(1, 0);
|
||
p_3d_w_mat.at<float>(2, 0) = p_3d_w.at<float>(2, 0);
|
||
|
||
Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
|
||
tcw = Tcw.rowRange(0, 3).col(3);
|
||
cv::Mat p_3d_c, p_3d_bev, p_uv_bev;
|
||
p_3d_c = Rcw * p_3d_w_mat + tcw; // 世界坐标转相机坐标
|
||
|
||
cv::Mat Rbc = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // 相机转bev的矩阵
|
||
p_3d_bev = Rbc * p_3d_c; // 相机转bev
|
||
p_3d_bev.at<float>(2, 0) = 1.0; // 归一
|
||
p_uv_bev = m_bev_K * p_3d_bev; // 过去bev相机坐标转过去bev像素坐标
|
||
u = p_uv_bev.at<float>(0, 0) / p_3d_bev.at<float>(2, 0);
|
||
v = p_uv_bev.at<float>(1, 0) / p_3d_bev.at<float>(2, 0);
|
||
if (u > 0 && u < 608 && v > 0 && v < 608)
|
||
{
|
||
mnInsight++;
|
||
mbInsight = true;
|
||
}
|
||
}
|
||
|
||
void Car::world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw) // 世界坐标转bev像素
|
||
{
|
||
for (auto p_3d_w : vm_3d_w)
|
||
{
|
||
cv::Mat Rcw, tcw, R_ij;
|
||
cv::Mat p_3d_w_mat = cv::Mat::zeros(3, 1, CV_32FC1);
|
||
cv::Point p_bev;
|
||
p_3d_w_mat.at<float>(0, 0) = p_3d_w.at<float>(0, 0); // 取bev像素坐标
|
||
p_3d_w_mat.at<float>(1, 0) = p_3d_w.at<float>(1, 0);
|
||
p_3d_w_mat.at<float>(2, 0) = p_3d_w.at<float>(2, 0);
|
||
|
||
Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
|
||
tcw = Tcw.rowRange(0, 3).col(3);
|
||
cv::Mat p_3d_c, p_3d_bev, p_uv_bev;
|
||
p_3d_c = Rcw * p_3d_w_mat + tcw; // 世界坐标转相机坐标
|
||
|
||
cv::Mat Rbc = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // 相机转bev的矩阵
|
||
p_3d_bev = Rbc * p_3d_c; // 相机转bev
|
||
p_3d_bev.at<float>(2, 0) = 1.0; // 归一
|
||
p_uv_bev = m_bev_K * p_3d_bev; // 过去bev相机坐标转过去bev像素坐标
|
||
p_bev.x = p_uv_bev.at<float>(0, 0) / p_3d_bev.at<float>(2, 0);
|
||
p_bev.y = p_uv_bev.at<float>(1, 0) / p_3d_bev.at<float>(2, 0);
|
||
vp_bev.push_back(p_bev);
|
||
}
|
||
}
|
||
|
||
void Car::update_untracked_Car()
|
||
{
|
||
mvLostTrackTimes++;
|
||
if (mvLostTrackTimes > 6)
|
||
{
|
||
mbBad = true;
|
||
}
|
||
else
|
||
{
|
||
|
||
// Step1: 速度不变
|
||
// Step2: 恒速运动，根据速度推算目标位置
|
||
int tmp_i = 0;
|
||
for (auto bboxPose : mvmBboxWorldPose)
|
||
{
|
||
|
||
mvmBboxWorldPose[tmp_i] = mRcl * mvmBboxWorldPose[tmp_i] + mtcl;
|
||
tmp_i++;
|
||
}
|
||
|
||
mCarWorldPose = mRcl * mCarWorldPose + mtcl;
|
||
}
|
||
}
|
||
|
||
void Car::setTrackedState(bool state)
|
||
{
|
||
mnTracked = state;
|
||
}
|
||
|
||
bool Car::isTracked()
|
||
{
|
||
return mnTracked;
|
||
}
|
||
|
||
void Car::updateVelocityAndPose(cv::Mat &pose, double &delta_t, vector<cv::Point> &vp_c, cv::Mat &Rwc, cv::Mat &twc)
|
||
{
|
||
cv::Mat pose_inv;
|
||
// matrix_inv(pose, pose_inv);
|
||
cv::Mat tmp_V, tmp_R, tmp_t;
|
||
// tmp_V = pose_inv * mCarWorldPose; // pose是当前位置，mCarWorldPose在更新时已经是上一帧的位置了
|
||
tmp_R = cv::Mat::eye(3, 3, CV_32FC1);
|
||
tmp_t = pose - mCarWorldPose;
|
||
|
||
Eigen::Vector2f yk(pose.at<float>(0, 0), pose.at<float>(2, 0));
|
||
if (this->mnVisual > 1)
|
||
{
|
||
kalmanfilter(yk, 0.1);
|
||
pose.at<float>(0, 0) = mmk_1(0);
|
||
pose.at<float>(2, 0) = mmk_1(1);
|
||
cv::Point2f tmp, tmp_v;
|
||
pose.at<float>(0, 0) = mmk_1(0);
|
||
pose.at<float>(2, 0) = mmk_1(1);
|
||
tmp.x = pose.at<float>(0, 0);
|
||
tmp.y = pose.at<float>(2, 0);
|
||
tmp_v.x = mmk_1(2);
|
||
tmp_v.y = mmk_1(3);
|
||
// cout << "第 " << mnCar_ID << " 辆车的位置为： " << tmp.x << " " << tmp.y << endl;
|
||
// cout << "第 " << mnCar_ID << " 辆车的位置为： " << pose.at<float>(0, 0) << " " << pose.at<float>(1, 0) << " " << pose.at<float>(2, 0) << endl;
|
||
mvvelocity.push_back(tmp_v);
|
||
mvpose.push_back(tmp);
|
||
}
|
||
|
||
/*
|
||
这里加入卡尔曼滤波，构造
|
||
mk，mtcl里是位移
|
||
*/
|
||
|
||
if (mdelta_w * 180 / 3.1415927 < 30) // 速度变化在合理范围内：TODO hong 速度变化计算
|
||
{
|
||
// Step1: 更新速度
|
||
// mmVelocity = tmp_V;
|
||
mtcl = tmp_t;
|
||
mRcl = tmp_R;
|
||
mtcl.at<float>(1, 0) = 0.0;
|
||
// Step2: 更新位置
|
||
mCarWorldPose = pose;
|
||
mvLostTrackTimes = 0;
|
||
bev2world(vp_c, mvmBboxWorldPose, Rwc, twc);
|
||
}
|
||
else
|
||
{
|
||
mvLostTrackTimes++;
|
||
if (mvLostTrackTimes > 6)
|
||
{
|
||
mbBad = true;
|
||
}
|
||
else
|
||
{
|
||
|
||
// Step1: 速度不变
|
||
// Step2: 恒速运动，根据速度推算目标位置
|
||
int tmp_i = 0;
|
||
for (auto bboxPose : mvmBboxWorldPose)
|
||
{
|
||
|
||
mvmBboxWorldPose[tmp_i] = mRcl * mvmBboxWorldPose[tmp_i] + mtcl;
|
||
tmp_i++;
|
||
}
|
||
|
||
mCarWorldPose = mRcl * mCarWorldPose + mtcl;
|
||
}
|
||
}
|
||
}
|
||
|
||
void Car::matrix_inv(cv::Mat T, cv::Mat T_inv)
|
||
{
|
||
T.copyTo(T_inv);
|
||
cv::Mat Rcw, Rwc, tcw, twc, Twc;
|
||
Rcw = T.rowRange(0, 3).colRange(0, 3);
|
||
Rwc = Rcw.t();
|
||
tcw = T.rowRange(0, 3).col(3);
|
||
twc = -Rcw.t() * tcw;
|
||
Rwc.copyTo(T_inv.rowRange(0, 3).colRange(0, 3));
|
||
twc.copyTo(T_inv.rowRange(0, 3).col(3));
|
||
}
|
||
|
||
void Car::updateTrackState(int ID)
|
||
{
|
||
if (ID - mnLastFrameID > 6)
|
||
{
|
||
if (mnVisual < 3)
|
||
{
|
||
mbErro = true;
|
||
}
|
||
mbBad = true;
|
||
}
|
||
}
|
||
|
||
void Car::updateState(vector<double> &bbox, cv::Mat &Rwc, cv::Mat &twc, double &delta_t)
|
||
{
|
||
cv::Mat tmp_CarWorldPose;
|
||
cv::Point carBev;
|
||
carBev.x = bbox[1];
|
||
carBev.y = bbox[2];
|
||
vector<cv::Point> vp_c;
|
||
get_bbx(bbox, vp_c);
|
||
bev2world(carBev, tmp_CarWorldPose, Rwc, twc); // 获得目标框世界坐标
|
||
mdelta_w = bbox[7] - myaw;
|
||
updateVelocityAndPose(tmp_CarWorldPose, delta_t, vp_c, Rwc, twc); // 更新速度并更新世界坐标
|
||
myaw = bbox[7];
|
||
Tow.rowRange(0, 3).colRange(0, 3) = Rwc * (cv::Mat_<float>(3, 3) << cos(myaw), 0, sin(myaw),
|
||
0, 1, 0,
|
||
-sin(myaw), 0, cos(myaw));
|
||
Tow.rowRange(0, 3).col(3) = mCarWorldPose;
|
||
}
|
||
|
||
void Car::updateObservations(int CurrentFrameId)
|
||
{
|
||
if (mbBad)
|
||
return;
|
||
mnVisual++;
|
||
|
||
mnLastFrameID = CurrentFrameId;
|
||
}
|
||
|
||
void Car::updatetraj()
|
||
{
|
||
cv::Point3f pose;
|
||
pose.x = mCarWorldPose.at<float>(0, 0);
|
||
pose.y = mCarWorldPose.at<float>(1, 0);
|
||
pose.z = mCarWorldPose.at<float>(2, 0);
|
||
mtraj.push_back(pose);
|
||
}
|
||
|
||
void Car::kalmanfilter(Eigen::Vector2f &yk, double delta_t)
|
||
{
|
||
if (mnVisual == 2)
|
||
{
|
||
mmk_1 << this->mCarWorldPose.at<float>(0, 0), this->mCarWorldPose.at<float>(2, 0), (yk[0] - this->mCarWorldPose.at<float>(0, 0)) / 0.1, (yk[1] - this->mCarWorldPose.at<float>(2, 0)) / 0.1;
|
||
}
|
||
if (mnVisual == 1)
|
||
{
|
||
cout << "error in kalmanfilter" << endl;
|
||
return;
|
||
}
|
||
|
||
Eigen::Matrix4f A;
|
||
A << 1, 0, delta_t, 0,
|
||
0, 1, 0, delta_t,
|
||
0, 0, 1, 0,
|
||
0, 0, 0, 1;
|
||
float q1c, q2c;
|
||
float sigma1, sigma2;
|
||
q1c = 1.0;
|
||
q2c = 1.0;
|
||
sigma1 = 0.5;
|
||
sigma2 = 0.5;
|
||
float delta_t3 = delta_t * delta_t * delta_t / 3;
|
||
float delta_t2 = delta_t * delta_t / 2;
|
||
|
||
Eigen::Matrix4f Q;
|
||
Q << q1c * delta_t3, 0, q1c * delta_t2, 0,
|
||
0, q2c * delta_t3, 0, q2c * delta_t2,
|
||
q1c * delta_t2, 0, q1c * delta_t, 0,
|
||
0, q2c * delta_t2, 0, q2c * delta_t;
|
||
|
||
Eigen::Vector4f mk_ = A * mmk_1;
|
||
Eigen::Matrix4f Pk_ = A * mPK_1 * A.transpose() + Q;
|
||
|
||
// 更新
|
||
Eigen::Vector2f vk = yk - mkalman_H * mk_;
|
||
Eigen::Matrix2f S_k = mkalman_H * Pk_ * mkalman_H.transpose() + mkalman_R;
|
||
Eigen::MatrixXf Kk = Pk_ * mkalman_H.transpose() * S_k.inverse();
|
||
|
||
mmk_1 = mk_ + Kk * vk;
|
||
|
||
Eigen::Matrix4f tmpawd = Pk_ - Kk * S_k * Kk.transpose();
|
||
mPK_1 = tmpawd;
|
||
// cout << "row = " << tmp_mmk_1.rows() << " col = " << tmp_mmk_1.cols() << endl;
|
||
}
|
||
|
||
void Car::Save_car_v()
|
||
{
|
||
|
||
std::string filename = "./Car_data/kalmanfilter/file_" + std::to_string(this->mnCar_ID) + ".txt";
|
||
|
||
if (this->mvpose.size() < 6)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 使用文件流打开文件
|
||
std::fstream fileStream(filename, std::ios::out | std::ios::app);
|
||
if (!fileStream.is_open())
|
||
{
|
||
std::ofstream newFile(filename);
|
||
// 如果需要在创建文件后写入，请在此处添加相应的写入代码
|
||
}
|
||
|
||
// 写入位置数据
|
||
for (size_t i = 0; i < mvpose.size(); i++)
|
||
{
|
||
fileStream << mvpose[i].x << " " << mvpose[i].y << " " << mvvelocity[i].x << " " << mvvelocity[i].y << std::endl;
|
||
}
|
||
|
||
// 关闭文件流
|
||
fileStream.close();
|
||
}
|
||
|
||
}
|