Skip to content

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

提交更新

parent de2fbc2f
Hide whitespace changes
Inline Side-by-side
Showing with 56 additions and 69 deletions
BaseTracker/BaseTracker.h
View file @ c19c1d84
......@@ -31,7 +31,7 @@ public:
int Run(const std::vector<std::vector<float> >& detections,int _no/*观测数量*/,int _ns/*状态数量*/,std::vector<uint64_t>& detectionsId,std::map<uint64_t,int>& updateId,std::vector<uint64_t>& lostId);
std::map<uint64_t, std::shared_ptr<T> > GetStates();
std::map<uint64_t, std::shared_ptr<T> >& GetStates();
void AssociateDetectionsToTrackers(const std::vector<std::vector<float> >& detections, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& matched, std::vector<int>& unmatched_det);
......@@ -171,19 +171,9 @@ int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, int
}
template<class T>
std::map<uint64_t, std::shared_ptr<T> > BaseTracker<T>::GetStates()
std::map<uint64_t, std::shared_ptr<T> >& BaseTracker<T>::GetStates()
{
std::map<uint64_t, std::shared_ptr<T> > ret;
for (auto& track : m_tracker)
{
if (track.second->IsValid())
{
ret[track.first] = track.second;
SDK_LOG(SDK_INFO, "id = %llu, tracker = %p", &(track.second->m_obj));
SDK_LOG(SDK_INFO, "id = %llu, ret = %p",&(ret[track.first]->m_obj));
}
}
return ret;
return m_tracker;
}
template<class T>
......
BaseTracker/Track3D.cpp
View file @ c19c1d84
......@@ -122,14 +122,6 @@ void Track3D::Predict()
{
if (kf_ == nullptr)
return;
if (m_obj == nullptr)
{
SDK_LOG(SDK_INFO, "m_obj = nullptr");
}
else
{
SDK_LOG(SDK_INFO, "m_obj = %p",m_obj.get());
}
point2d pos = {};
pos.x = kf_->x_[0];
pos.y = kf_->x_[1];
......
TrackingRos.cpp
View file @ c19c1d84
......@@ -318,7 +318,7 @@ void TrackingRos::ThreadTrackingProcess()
m_tracker.Run(input,7,10, detectionsId, updateId, lostId);
uint64_t rTime = GetCurTime() - begin;
//SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
std::map<uint64_t, std::shared_ptr<Track3D> > trackers = m_tracker.GetStates();
std::map<uint64_t, std::shared_ptr<Track3D> >& trackers = m_tracker.GetStates();
static int count = 0;
static int calcCount = 0;
count++;
......@@ -344,13 +344,15 @@ void TrackingRos::ThreadTrackingProcess()
{
jfx_common_msgs::det_tracking obj = {};
jfx_common_msgs::det_tracking input_obj = {};
int is_need_send = 0;//是否需要发送
if (updateId.find(iter.first) != updateId.end())
{
obj = objsPtr->array[updateId[iter.first]];
input_obj = obj;
std::vector<float> data;
if (iter.second->GetStateData(data) == 0)
if (iter.second->IsValid() && iter.second->GetStateData(data) == 0)
{
is_need_send = 1;//需要发送
obj.x = data[0];
obj.y = data[1];
obj.z = data[2];
......@@ -373,15 +375,15 @@ void TrackingRos::ThreadTrackingProcess()
//SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f",obj.Lat,obj.Long);
}
iter.second->m_obj = std::make_shared<jfx_common_msgs::det_tracking>(obj);
SDK_LOG(SDK_INFO, "id = %llu, m_obj = %p",iter.first,iter.second->m_obj.get());
}
else
{
if(iter.second->m_obj)
obj = *(iter.second->m_obj);
std::vector<float> data;
if (iter.second->GetStateData(data) == 0)
if (iter.second->IsValid() && iter.second->GetStateData(data) == 0)
{
is_need_send = 1;//需要发送
obj.x = data[0];
obj.y = data[1];
obj.z = data[2];
......@@ -404,52 +406,55 @@ void TrackingRos::ThreadTrackingProcess()
//SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
}
}
//修正rot_y值,变为角度
while (obj.rot_y >= _PI_)
if (is_need_send == 1)
{
obj.rot_y -= _PI_ * 2;
}
while (obj.rot_y < -_PI_)
{
obj.rot_y += _PI_ * 2;
}
float rot_y_angle = obj.rot_y;
obj.rot_y = to360(obj.rot_y, m_lidar_x_angle);
//SDK_LOG(SDK_INFO, "rot_y = %f, rot_y_angle = %f", rot_y_angle, rot_y_a);
std::vector<float> predict;
if (iter.second->GetPredictData(predict) == 0)
{
//SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%f,%f,%f,%f,%f,%f,%f) predict(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
// obj.frame, iter.first, objsPtr->array[updateId[iter.first]].x, obj sPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
// objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
// predict[0], predict[1], predict[2], predict[4], predict[5], predict[6], predict[3],
// obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, iter.second->GetProb());
//date,frame,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon
SDK_IDX_LOG(1, SDK_INFO, "%s,%d,%llu,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.10f,%.10f",
GetTimeStr(obj.frame).c_str(),m_frameNum, obj.frame, 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.x, input_obj.y, input_obj.z, input_obj.l, input_obj.w, input_obj.h,
predict[0], predict[1], predict[2], predict[4], predict[5], predict[6],
obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y,obj.v_x,obj.v_y,obj.v_z,obj.Lat,obj.Long);
}
//修正rot_y值,变为角度
while (obj.rot_y >= _PI_)
{
obj.rot_y -= _PI_ * 2;
}
while (obj.rot_y < -_PI_)
{
obj.rot_y += _PI_ * 2;
}
float rot_y_angle = obj.rot_y;
obj.rot_y = to360(obj.rot_y, m_lidar_x_angle);
//SDK_LOG(SDK_INFO, "rot_y = %f, rot_y_angle = %f", rot_y_angle, rot_y_a);
std::vector<float> predict;
if (iter.second->GetPredictData(predict) == 0)
{
//SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%f,%f,%f,%f,%f,%f,%f) predict(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
// obj.frame, iter.first, objsPtr->array[updateId[iter.first]].x, obj sPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
// objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
// predict[0], predict[1], predict[2], predict[4], predict[5], predict[6], predict[3],
// obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, iter.second->GetProb());
//date,frame,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon
SDK_IDX_LOG(1, SDK_INFO, "%s,%d,%llu,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.10f,%.10f",
GetTimeStr(obj.frame).c_str(), m_frameNum, obj.frame, 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.x, input_obj.y, input_obj.z, input_obj.l, input_obj.w, input_obj.h,
predict[0], predict[1], predict[2], predict[4], predict[5], predict[6],
obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, obj.v_x, obj.v_y, obj.v_z, obj.Lat, obj.Long);
}
jsk_recognition_msgs::BoundingBox box = {};
box.label = obj.obj_id;
box.value = 2;
box.header.frame_id = "Pandar64";
box.dimensions.x = obj.l;
box.dimensions.y = obj.w;
box.dimensions.z = obj.h;
box.pose.position.x = obj.x;
box.pose.position.y = obj.y;
box.pose.position.z = obj.z;
Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond R_quat(V3);
box.pose.orientation.x = R_quat.x();
box.pose.orientation.y = R_quat.y();
box.pose.orientation.z = R_quat.z();
box.pose.orientation.w = R_quat.w();
sendBoxes.boxes.emplace_back(box);
sendObjs.array.emplace_back(obj);
jsk_recognition_msgs::BoundingBox box = {};
box.label = obj.obj_id;
box.value = 2;
box.header.frame_id = "Pandar64";
box.dimensions.x = obj.l;
box.dimensions.y = obj.w;
box.dimensions.z = obj.h;
box.pose.position.x = obj.x;
box.pose.position.y = obj.y;
box.pose.position.z = obj.z;
Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond R_quat(V3);
box.pose.orientation.x = R_quat.x();
box.pose.orientation.y = R_quat.y();
box.pose.orientation.z = R_quat.z();
box.pose.orientation.w = R_quat.w();
sendBoxes.boxes.emplace_back(box);
sendObjs.array.emplace_back(obj);
}
}
//sendObjs.cloud = objsPtr->cloud;
//SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
......
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