Skip to content

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

提交测试

parent ba368567
Hide whitespace changes
Inline Side-by-side
Showing with 77 additions and 38 deletions
TrackingRos.cpp
View file @ d23a693b
......@@ -25,37 +25,16 @@ 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)
{
float c = cosf(t);
float s = sinf(t);
//void my_compute_box_3d(Eigen::Vector3f center, float heading, Eigen::Vector3f size, Eigen::Matrix<float, 8, 3>& res_corners_3d)
//{
// Eigen::Matrix<float, 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;
// svTmp << size[0], size[1], size[2], 1;
// Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
// //Eigen::Matrix3f rot;
// //rotz(heading, rot);
// //Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
// //for (int i = 0; i < 3; i++) {
// // for (int j = 0; j < 3; j++) {
// // Trans(i, j) = rot(i, j);
// // }
// //}
// //Trans(0, 3) = center[0];
// //Trans(1, 3) = center[1];
// //Trans(2, 3) = center[2];
//
// auto tmp4x4 = S * corners_3d;
// auto world_corners_3d = Trans * tmp4x4;
// res_corners_3d = world_corners_3d.block(0, 0, 3, 8).transpose();
//}
void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, std::vector<std::vector<float>>& kitti2origin, float& gx, float& gy)
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)
{
Eigen::Matrix<float, 4, 8> corners_3d;
corners_3d << -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5,
......@@ -64,8 +43,19 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
1, 1, 1, 1, 1, 1, 1, 1;
Eigen::Vector4f svTmp;
svTmp << x, y, z, 1;
Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
svTmp << size[0], size[1], size[2], 1;
Eigen::Matrix4f S = svTmp.asDiagonal();
Eigen::Matrix3f rot;
rotz(heading, rot);
Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
Trans(i, j) = rot(i, j);
}
}
Trans(0, 3) = center[0];
Trans(1, 3) = center[1];
Trans(2, 3) = center[2];
Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
for (int i = 0; i < 4; i++) {
......@@ -74,24 +64,38 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
}
}
auto tmp4x8 = S * corners_3d;
auto origin4x8 = Kit2Ori * tmp4x8;
auto res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
auto tmp4x4 = S * corners_3d;
auto world_corners_3d = Trans * tmp4x4;
world_corners_3d = Kit2Ori * world_corners_3d;
res_corners_3d = world_corners_3d.block(0, 0, 3, 8).transpose();
}
void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vector<float>>& trans, std::vector<std::vector<float>>& kitti2origin,float& gx,float& 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;
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::Vector3f head_pnt = (res3 + res2) / 2;
Eigen::Vector3f tail_pnt = (res0 + res3) / 2;
Eigen::Vector3f 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;
Eigen::Vector4f tranTmp;
tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
for (int i = 0; i < 4; i++) {
......@@ -104,6 +108,41 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
gx = world_pos[0];
gy = world_pos[1];
//Eigen::Matrix<float, 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;
//svTmp << x, y, z, 1;
//Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
//Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
//for (int i = 0; i < 4; i++) {
// for (int j = 0; j < 4; j++) {
// Kit2Ori(i, j) = kitti2origin[i][j];
// }
//}
//auto tmp4x8 = S * corners_3d;
//auto origin4x8 = Kit2Ori * tmp4x8;
//auto res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
//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 head_pnt = (res3 + res2) / 2;
//Eigen::Vector3f tail_pnt = (res0 + res3) / 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;
}
TrackingRos::~TrackingRos()
......@@ -298,7 +337,7 @@ void TrackingRos::ThreadTrackingProcess()
obj.obj_id = iter.first;
float gx = 0.0f;
float gy = 0.0f;
CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, m_kitti2origin, gx, gy);
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};
jfx::Array pos = jfx::Inverse(gpos);
......@@ -328,7 +367,7 @@ void TrackingRos::ThreadTrackingProcess()
obj.obj_id = iter.first;
float gx = 0.0f;
float gy = 0.0f;
CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, m_kitti2origin, gx, gy);
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 };
jfx::Array pos = jfx::Inverse(gpos);
......
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