Skip to content

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

提交更新

parent 286ad7f2
Hide whitespace changes
Inline Side-by-side
Showing with 27 additions and 27 deletions
TrackingRos.cpp
View file @ 4b8a92f0
......@@ -26,33 +26,33 @@ float to360(float rot_y,float lidar_x_angle)
return angle;
}
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 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)
......
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