zqjia/HCY_Graduation_Project
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
HCY_Graduation_Project/src/Car.cpp

480 lines
16 KiB
C++
Raw Normal View History

the final version
2024-06-21 15:44:42 +08:00
#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();
}
}
Reference in New Issue Copy Permalink