Skip to content

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

提交保存csv数据

parent 7012979b
Hide whitespace changes
Inline Side-by-side
Showing with 52 additions and 14 deletions
BaseTracker/Track3D.cpp
View file @ f1f013e5
......@@ -234,10 +234,10 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
{
deta += _PI_ * 2;
}
if (abs(deta) > ANGLE_CHANGE_MAX)
{
deta = deta > 0 ? ANGLE_CHANGE_MAX : -ANGLE_CHANGE_MAX;
}
//if (abs(deta) > ANGLE_CHANGE_MAX)
//{
// deta = deta > 0 ? ANGLE_CHANGE_MAX : -ANGLE_CHANGE_MAX;
//}
angle = x_angle + deta;
out = data;
out[3] = angle;
......
TrackingRos.cpp
View file @ f1f013e5
......@@ -75,6 +75,9 @@ void TrackingRos::Init(ros::NodeHandle& nh)
auto cfg = config["TRACKING"];
m_trans = cfg["TRANS"].as<std::vector<std::vector<float>>>();
ADD_DEF_LOG(1, "tracking.csv");
SDK_IDX_LOG(1, SDK_INFO, "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");
//m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
......@@ -204,9 +207,11 @@ void TrackingRos::ThreadTrackingProcess()
for (auto& iter : trackers)
{
jfx_common_msgs::det_tracking obj = {};
jfx_common_msgs::det_tracking input_obj = {};
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)
{
......@@ -221,15 +226,6 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
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, objsPtr->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());
}
float gx = 0.0f;
float gy = 0.0f;
CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
......@@ -283,6 +279,21 @@ void TrackingRos::ThreadTrackingProcess()
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, objsPtr->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,%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(), 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;
......
common/Component.cpp
View file @ f1f013e5
......@@ -56,3 +56,20 @@ std::string GetMatrixStr(float** data_ptr, int col, int row)
}
return str;
}
std::string GetTimeStr(uint64_t timestamp)
{
time_t tt = timestamp / 1000;
int misc = timestamp % 1000;
struct tm* ptr;
ptr = localtime(&tt);
// printf("time: %d \n", tt);
char str[80];
strftime(str, sizeof(str), "%Y-%m-%d %H:%M:%S", ptr);
//2018-09-19 16:01:37.517
char tStr[128] = {};
sprintf(tStr, ".%d", misc);
std::string timeStr = std::string(str) + std::string(tStr);
return timeStr;
}
\ No newline at end of file
common/Component.h
View file @ f1f013e5
......@@ -5,6 +5,9 @@
uint64_t GetCurTime();
std::string GetTimeStr(uint64_t timestamp);
std::string GetMatrixStr(const float* data_ptr, int col, int row);
std::string GetMatrixStr(const std::vector<std::vector<float>>& data_ptr, int col, int row);
std::string GetMatrixStr(float** data_ptr, int col, int row);
log/jfx_log.cpp
View file @ f1f013e5
......@@ -48,7 +48,13 @@ int AddLogFileSys(const char* file, int& fileIdx)
return -1;
fileIdx = 1;
std::string path = GetRootDir();
std::string dir = path + "/" + file;
time_t tt = time(NULL);
tm* t = localtime(&tt);
char logName[1024] = {};
sprintf(logName, "%04d%02d%02d-%02d%02d%02d_%s", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec, file);
std::string csvFile = std::string(logName);
std::string dir = path + "/" + csvFile;
LOG(INFO) << "AddLogFileSys save file = " << dir;
#ifdef _WIN32
if (fopen_s(&g_file, dir.c_str(), "wt") != 0)
......
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