Skip to content

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

修改iou的bug错误

parent d2c49dc4
Hide whitespace changes
Inline Side-by-side
Showing with 58 additions and 73 deletions
BaseTracker/Track3D.cpp
View file @ 7c435686
......@@ -241,7 +241,7 @@ double Track3D::CalculateIou(const std::vector<float>& data)
if (interHeight > 0)
{
double inter3D = area * interHeight;
iou_3d = inter3D / (states[3] * states[4] * states[5] + data[4] * data[5] * data[6] - inter3D);
iou_3d = inter3D / (states[4] * states[5] * states[6] + data[5] * data[6] * data[7] - inter3D);
}
}
//SDK_LOG(SDK_INFO, "CuboidIoU time = %llu", GetCurTime() - begin);
......
TrackingRosEx.cpp
View file @ 7c435686
......@@ -234,13 +234,13 @@ TrackingRos::~TrackingRos()
void TrackingRos::Init(ros::NodeHandle& nh)
{
float x, y;
get_camera_tolidar_loc(400, 400, x, y);
SDK_LOG(SDK_INFO, "get_camera_tolidar_loc x = %f, y = %f",x,y);
get_camera_right_tolidar_loc(400, 400, x, y);
SDK_LOG(SDK_INFO, "get_camera_right_tolidar_loc x = %f, y = %f", x, y);
get_camera_left_tolidar_loc(400, 400, x, y);
SDK_LOG(SDK_INFO, "get_camera_left_tolidar_loc x = %f, y = %f", x, y);
//float x, y;
//get_camera_tolidar_loc(400, 400, x, y);
//SDK_LOG(SDK_INFO, "get_camera_tolidar_loc x = %f, y = %f",x,y);
//get_camera_right_tolidar_loc(400, 400, x, y);
//SDK_LOG(SDK_INFO, "get_camera_right_tolidar_loc x = %f, y = %f", x, y);
//get_camera_left_tolidar_loc(400, 400, x, y);
//SDK_LOG(SDK_INFO, "get_camera_left_tolidar_loc x = %f, y = %f", x, y);
std::string folder,yaml;
nh.param<std::string>("project_path", folder, "/home/nvidia/catkin_ws_M1/src/jfxrosperceiver");
......@@ -305,55 +305,52 @@ void TrackingRos::Init(ros::NodeHandle& nh)
m_isTrackingRun = true;
m_trackingThread = std::thread(&TrackingRos::ThreadTrackingProcess, this);
Eigen::MatrixXf mat(2,2);
mat(0, 0) = 0.1f;
mat(0, 1) = 0.2f;
mat(1, 0) = 0.3f;
mat(1, 1) = 0.4f;
float* f = mat.data();
f[3] = 0.5f;
float m[4];
memcpy(m, f, sizeof(float) * 4);
SDK_LOG(SDK_INFO, "data = %f,%f,%f,%f", m[0], m[1], m[2], m[3]);
std::vector<point2d> tests;
tests.push_back(point2d(1, 1));
tests.push_back(point2d(2, 2));
tests.push_back(point2d(3, 3));
tests.push_back(point2d(4, 4));
tests.push_back(point2d(5, 5));
double angle = correct_angle(tests);
SDK_LOG(SDK_INFO, "test correct_angle = %f", angle);
tests.clear();
tests.push_back(point2d(5, 5));
tests.push_back(point2d(4, 4));
tests.push_back(point2d(3, 3));
tests.push_back(point2d(2, 2));
tests.push_back(point2d(1, 1));
angle = correct_angle(tests);
SDK_LOG(SDK_INFO, "test2 correct_angle = %f", angle);
tests.clear();
tests.push_back(point2d(-1, 1));
tests.push_back(point2d(-2, 2));
tests.push_back(point2d(-3, 3));
tests.push_back(point2d(-4, 4));
tests.push_back(point2d(-5, 5));
angle = correct_angle(tests);
SDK_LOG(SDK_INFO, "test3 correct_angle = %f", angle);
tests.clear();
tests.push_back(point2d(-5, 5));
tests.push_back(point2d(-4, 4));
tests.push_back(point2d(-3, 3));
tests.push_back(point2d(-2, 2));
tests.push_back(point2d(-1, 1));
angle = correct_angle(tests);
SDK_LOG(SDK_INFO, "test4 correct_angle = %f", angle);
//Eigen::MatrixXf mat(2,2);
//mat(0, 0) = 0.1f;
//mat(0, 1) = 0.2f;
//mat(1, 0) = 0.3f;
//mat(1, 1) = 0.4f;
//float* f = mat.data();
//f[3] = 0.5f;
//float m[4];
//memcpy(m, f, sizeof(float) * 4);
//SDK_LOG(SDK_INFO, "data = %f,%f,%f,%f", m[0], m[1], m[2], m[3]);
//std::vector<point2d> tests;
//tests.push_back(point2d(1, 1));
//tests.push_back(point2d(2, 2));
//tests.push_back(point2d(3, 3));
//tests.push_back(point2d(4, 4));
//tests.push_back(point2d(5, 5));
//double angle = correct_angle(tests);
//SDK_LOG(SDK_INFO, "test correct_angle = %f", angle);
//tests.clear();
//tests.push_back(point2d(5, 5));
//tests.push_back(point2d(4, 4));
//tests.push_back(point2d(3, 3));
//tests.push_back(point2d(2, 2));
//tests.push_back(point2d(1, 1));
//angle = correct_angle(tests);
//SDK_LOG(SDK_INFO, "test2 correct_angle = %f", angle);
//tests.clear();
//tests.push_back(point2d(-1, 1));
//tests.push_back(point2d(-2, 2));
//tests.push_back(point2d(-3, 3));
//tests.push_back(point2d(-4, 4));
//tests.push_back(point2d(-5, 5));
//angle = correct_angle(tests);
//SDK_LOG(SDK_INFO, "test3 correct_angle = %f", angle);
//tests.clear();
//tests.push_back(point2d(-5, 5));
//tests.push_back(point2d(-4, 4));
//tests.push_back(point2d(-3, 3));
//tests.push_back(point2d(-2, 2));
//tests.push_back(point2d(-1, 1));
//angle = correct_angle(tests);
//SDK_LOG(SDK_INFO, "test4 correct_angle = %f", angle);
}
void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息
......@@ -444,7 +441,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
continue;
if (_GET_VALID_VALUE((unsigned char)0, m_recvMsgBit))
{
SDK_LOG(SDK_INFO, "send cloud obj x = %f, y = %f",obj.x,obj.y);
//SDK_LOG(SDK_INFO, "send cloud obj x = %f, y = %f",obj.x,obj.y);
objsPtr->array.push_back(obj);
}
}
......@@ -570,7 +567,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
}
if (_GET_VALID_VALUE((unsigned char)(i + 1), m_recvMsgBit))
{
SDK_LOG(SDK_INFO, "send camera i = %d, x = %f,y = %f",i, objMsg.x, objMsg.y);
//SDK_LOG(SDK_INFO, "send camera i = %d, x = %f,y = %f",i, objMsg.x, objMsg.y);
objsPtr->array.push_back(objMsg);
}
}
......@@ -624,7 +621,7 @@ void TrackingRos::ThreadTrackingProcess()
nvtxRangePush("ThreadTrackingProcess");
#endif
m_frameNum++;
SDK_LOG(SDK_INFO, "get objs size = %d,m_frameNum = %d",objsPtr->array.size(), m_frameNum);
//SDK_LOG(SDK_INFO, "get objs size = %d,m_frameNum = %d",objsPtr->array.size(), m_frameNum);
std::vector< std::vector<float> > input;
for (int i = 0; i < objsPtr->array.size(); i++)
{
......@@ -639,10 +636,6 @@ void TrackingRos::ThreadTrackingProcess()
data.push_back(obj.w);
data.push_back(obj.h);
input.emplace_back(data);
if (obj.type == 7)
{
SDK_LOG(SDK_INFO, "input========================m_frameNun = %d,[%f,%f,%f,%f,%f,%f,%f]", m_frameNum, obj.x, obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y);
}
}
std::vector<uint64_t> detectionsId;
std::map<uint64_t, int> updateId;
......@@ -729,10 +722,6 @@ void TrackingRos::ThreadTrackingProcess()
if(iter.second->m_obj)
obj = *(iter.second->m_obj);
std::vector<float> data;
if (obj.type == 7)
{
SDK_LOG(SDK_INFO, "update camera msg id = %llu x = %f, y = %f",obj.obj_id,obj.x,obj.y);
}
if (iter.second->IsValid() && iter.second->GetStateData(data) == 0)
{
is_need_send = 1;//需要发送
......@@ -775,7 +764,7 @@ void TrackingRos::ThreadTrackingProcess()
std::vector<float> predict;
if (iter.second->GetPredictData(predict) == 0)
{
SDK_LOG(SDK_INFO, "send obj m_frameNum = %d,id = %llu, x = %f,y = %f, type = %d", m_frameNum, iter.first,obj.x,obj.y,obj.type);
//SDK_LOG(SDK_INFO, "send obj m_frameNum = %d,id = %llu, x = %f,y = %f, type = %d", m_frameNum, iter.first,obj.x,obj.y,obj.type);
//SDK_LOG(SDK_INFO, "send obj m_frameNum = %d,id = %llu,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.10f,%.10f",
// m_frameNum, iter.first, input_obj.rot_y, input_obj.rot_y * 180 / _PI_, predict[3], predict[3] * 180 / _PI_, rot_y_angle, rot_y_angle * 180 / _PI_,
// input_obj.type, input_obj.x, input_obj.y, input_obj.z, input_obj.l, input_obj.w, input_obj.h,
......@@ -805,10 +794,6 @@ void TrackingRos::ThreadTrackingProcess()
box.pose.orientation.z = R_quat.z();
//SDK_LOG(SDK_INFO, "rot_y_angle = %f", rot_y_angle);
box.pose.orientation.w = R_quat.w();
if (obj.type == 7)
{
SDK_LOG(SDK_INFO, "sendbox========================= x = %f,y = %f",obj.x,obj.y);
}
sendBoxes.boxes.emplace_back(box);
sendObjs.array.emplace_back(obj);
}
......
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