Skip to content

J
jfx_kalman_filter_src
Commit 2cbc65cb authored by Haoshuang's avatar Haoshuang
Browse files
Options

Update TrackingRos.cpp for shanghai loc evaluation

parent f7ddd0ba
Hide whitespace changes
Inline Side-by-side
Showing with 8 additions and 8 deletions
TrackingRos.cpp
View file @ 2cbc65cb
......@@ -102,17 +102,17 @@ void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wg
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::Vector3d head_pnt = (res3 + res2) / 2;
// Eigen::Vector3d tail_pnt = (res0 + res3) / 2;
Eigen::Vector3d center_pnt = (res0 + res6) / 2;
Eigen::Vector3d head_pnt = (res1 + res2) / 2;
Eigen::Vector3d tail_pnt = (res0 + res3) / 2;
// Eigen::Vector3d center_pnt = (res0 + res6) / 2;
// double inter_len = 5.0f;
// double total_len = 6.0f;
// Eigen::Vector3d lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
double inter_len = 3.5f;
double total_len = 5.03f;
Eigen::Vector3d lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
Eigen::Vector4d tranTmp;
tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
// tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
// tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
Eigen::Matrix4d Trans2station = Eigen::Matrix4d::Zero();
for (int i = 0; i < 4; i++)
......
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