Skip to content

J
jfx_kalman_filter_src
Commit 754406bb authored by oscar's avatar oscar
Browse files
Options

提交测试

parent 4f0df770
Hide whitespace changes
Inline Side-by-side
Showing with 36 additions and 36 deletions
TrackingRos.cpp
View file @ 754406bb
......@@ -25,29 +25,29 @@ float to360(float rot_y,float lidar_x_angle)
//SDK_LOG(SDK_INFO, "angle = %f,angle_y = %f", angle, angle/180*3.1415926);
return angle;
}
void rotz(const float t, Eigen::Matrix3f& rotm)
void rotz(const double t, Eigen::Matrix3d& rotm)
{
float c = cosf(t);
float s = sinf(t);
double c = cosf(t);
double s = sinf(t);
rotm << c, -s, 0,
s, c, 0,
0, 0, 1;
}
void my_compute_box_3d(Eigen::Vector3f center, float heading, Eigen::Vector3f size, std::vector<std::vector<float>>& kitti2origin, Eigen::Matrix<float, 8, 3>& res_corners_3d)
void my_compute_box_3d(Eigen::Vector3d center, double heading, Eigen::Vector3d size, std::vector<std::vector<double>>& kitti2origin, Eigen::Matrix<double, 8, 3>& res_corners_3d)
{
Eigen::Matrix<float, 4, 8> corners_3d;
Eigen::Matrix<double, 4, 8> corners_3d;
corners_3d << -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5,
0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5,
0.5, 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5,
1, 1, 1, 1, 1, 1, 1, 1;
Eigen::Vector4f svTmp;
Eigen::Vector4d svTmp;
svTmp << size[0], size[1], size[2], 1;
Eigen::Matrix4f S = svTmp.asDiagonal();
Eigen::Matrix3f rot;
Eigen::Matrix4d S = svTmp.asDiagonal();
Eigen::Matrix3d rot;
rotz(heading, rot);
Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
Eigen::Matrix4d Trans = Eigen::Matrix4d::Zero();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
Trans(i, j) = rot(i, j);
......@@ -57,7 +57,7 @@ void my_compute_box_3d(Eigen::Vector3f center, float heading, Eigen::Vector3f si
Trans(1, 3) = center[1];
Trans(2, 3) = center[2];
Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
Eigen::Matrix4d Kit2Ori = Eigen::Matrix4d::Zero();
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
Kit2Ori(i, j) = kitti2origin[i][j];
......@@ -71,33 +71,33 @@ void my_compute_box_3d(Eigen::Vector3f center, float heading, Eigen::Vector3f si
}
void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vector<float>>& trans, std::vector<std::vector<float>>& kitti2origin,float& gx,float& gy)
void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vector<double>>& trans, std::vector<std::vector<double>>& kitti2origin,double& gx,double& gy)
{
Eigen::Vector3f center(obj.x, obj.y, obj.z);
float heading = obj.rot_y;
Eigen::Vector3f size(obj.l, obj.w, obj.h);
Eigen::Matrix<float, 8, 3> res_corners_3d;
Eigen::Vector3d center(obj.x, obj.y, obj.z);
double heading = obj.rot_y;
Eigen::Vector3d size(obj.l, obj.w, obj.h);
Eigen::Matrix<double, 8, 3> res_corners_3d;
my_compute_box_3d(center, heading, size, kitti2origin, res_corners_3d);
Eigen::Vector3f res0(res_corners_3d(0, 0), res_corners_3d(0, 1), res_corners_3d(0, 2));
Eigen::Vector3f res1(res_corners_3d(1, 0), res_corners_3d(1, 1), res_corners_3d(1, 2));
Eigen::Vector3f res2(res_corners_3d(2, 0), res_corners_3d(2, 1), res_corners_3d(2, 2));
Eigen::Vector3f res3(res_corners_3d(3, 0), res_corners_3d(3, 1), res_corners_3d(3, 2));
Eigen::Vector3f res6(res_corners_3d(6, 0), res_corners_3d(6, 1), res_corners_3d(6, 2));
Eigen::Vector3d res0(res_corners_3d(0, 0), res_corners_3d(0, 1), res_corners_3d(0, 2));
Eigen::Vector3d res1(res_corners_3d(1, 0), res_corners_3d(1, 1), res_corners_3d(1, 2));
Eigen::Vector3d res2(res_corners_3d(2, 0), res_corners_3d(2, 1), res_corners_3d(2, 2));
Eigen::Vector3d res3(res_corners_3d(3, 0), res_corners_3d(3, 1), res_corners_3d(3, 2));
Eigen::Vector3d res6(res_corners_3d(6, 0), res_corners_3d(6, 1), res_corners_3d(6, 2));
Eigen::Vector3f head_pnt = (res3 + res2) / 2;
Eigen::Vector3f tail_pnt = (res0 + res3) / 2;
Eigen::Vector3f center_pnt = (res0 + res6) / 2;
//Eigen::Vector3d head_pnt = (res3 + res2) / 2;
//Eigen::Vector3d tail_pnt = (res0 + res3) / 2;
Eigen::Vector3d center_pnt = (res0 + res6) / 2;
float inter_len = 0.413f;
float total_len = 4.6f;
Eigen::Vector3f lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
//double inter_len = 0.413f;
//double total_len = 4.6f;
//Eigen::Vector3d lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
Eigen::Vector4f tranTmp;
Eigen::Vector4d tranTmp;
tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
Eigen::Matrix4d Trans = Eigen::Matrix4d::Zero();
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
Trans(i, j) = trans[i][j];
......@@ -177,10 +177,10 @@ void TrackingRos::Init(ros::NodeHandle& nh)
auto cfg = config["TRACKING"];
m_trans = cfg["TRANS"].as<std::vector<std::vector<float>>>();
m_trans = cfg["TRANS"].as<std::vector<std::vector<double>>>();
auto cfg2 = config["POINTPILLARS"];
m_kitti2origin = cfg2["KITTI2ORIGIN"].as<std::vector<std::vector<float>>>();
m_kitti2origin = cfg2["KITTI2ORIGIN"].as<std::vector<std::vector<double>>>();
SDK_LOG(SDK_INFO, "trans = [%s]", GetMatrixStr(m_trans, 4, 4).c_str());
SDK_LOG(SDK_INFO, "kitti2origin = [%s]", GetMatrixStr(m_kitti2origin,4,4).c_str());
......@@ -335,8 +335,8 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
float gx = 0.0f;
float gy = 0.0f;
double gx = 0.0f;
double gy = 0.0f;
CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
//SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f",obj.x,obj.y,gx,gy,iter.second->GetProb());
jfx::Array gpos = {gx,gy};
......@@ -365,8 +365,8 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
float gx = 0.0f;
float gy = 0.0f;
double gx = 0.0f;
double gy = 0.0f;
CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
//SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
jfx::Array gpos = { gx,gy };
......
TrackingRos.h
View file @ 754406bb
......@@ -43,8 +43,8 @@ public:
ros::Publisher m_pubCloud;//发送点云数据
ros::Publisher m_pubBoundingBoxes;//发送3D框信息
std::vector<std::vector<float>> m_trans;
std::vector<std::vector<float>> m_kitti2origin;
std::vector<std::vector<double>> m_trans;
std::vector<std::vector<double>> m_kitti2origin;
int m_frameNum = 0;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment