Skip to content

J
jfx_kalman_filter_src
Commit 6c390320 authored by haoshuang's avatar haoshuang
Browse files
Options

modify gauss to utm

parent 88e6e2b6
Hide whitespace changes
Inline Side-by-side
Showing with 96 additions and 9 deletions
TrackingRos.cpp
View file @ 6c390320
......@@ -6,6 +6,9 @@
#include <math.h>
#include "LogBase.h"
#include <yaml-cpp/yaml.h>
#ifdef _ABUZHABI_
#include "uv_utm_lla.h"
#endif
#include "matrix.h"
#include <eigen3/Eigen/Dense>
#ifdef _USING_NSIGHT_
......@@ -80,6 +83,64 @@ void my_compute_box_3d(Eigen::Vector3d center, double heading, Eigen::Vector3d s
res_corners_3d = world_origin_3d.block(0, 0, 3, 8).transpose();
}
void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wgs84_station, std::vector<double> &UTM_station, std::vector<std::vector<double>> &trans2station, std::vector<std::vector<double>> &kitti2origin, double &lon, double &lat)
{
Eigen::Vector3d center(obj.x, obj.y, obj.z);
double heading = obj.rot_y;
Eigen::Vector3d size(obj.l, obj.w, obj.h);
Eigen::Matrix<double, 8, 3> res_corners_3d;
my_compute_box_3d(center, heading, size, kitti2origin, res_corners_3d);
Eigen::Vector3d res0(res_corners_3d(0, 0), res_corners_3d(0, 1), res_corners_3d(0, 2));
Eigen::Vector3d res1(res_corners_3d(1, 0), res_corners_3d(1, 1), res_corners_3d(1, 2));
Eigen::Vector3d res2(res_corners_3d(2, 0), res_corners_3d(2, 1), res_corners_3d(2, 2));
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;
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;
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;
Eigen::Matrix4d Trans2station = Eigen::Matrix4d::Zero();
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
Trans2station(i, j) = trans2station[i][j];
}
}
auto local_pos = Trans2station * tranTmp;
std::vector<double> UTM_pos = { local_pos(0) + UTM_station[0],
local_pos(1) + UTM_station[1],
local_pos(2) + UTM_station[2] };
std::vector<double> lonlatH_pos = {0,0,UTM_pos[2]};
jfx::UVUTMLLA * UVUTMLLAPtr = new jfx::UVUTMLLA;
int ret = UVUTMLLAPtr->Init_set_zone(UTM_station[1]);
if (0 != ret)
{
std::cout << "UTM Init Error" << std::endl;
}
UVUTMLLAPtr->UTM2WGS84(UTM_pos,lonlatH_pos);
lon = lonlatH_pos[0];
lat = lonlatH_pos[1];
delete (UVUTMLLAPtr);
UVUTMLLAPtr = nullptr;
}
void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vector<double>>& trans, std::vector<std::vector<double>>& kitti2origin,double& gx,double& gy)
{
......@@ -182,8 +243,6 @@ void TrackingRos::Init(ros::NodeHandle& nh)
if (ret != 0)return;
m_config = parser.entry["TRACKING_YAML_NODE"];
config = m_config;
m_nodeName = parser.node_name;
SDK_LOG(SDK_INFO, "m_nodeName = %s", m_nodeName.c_str());
}
else
{
......@@ -468,6 +527,18 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
#ifdef _ABUZHABI_
double lon = 0.0f;
double lat = 0.0f;
// CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat);
// SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
// jfx::Array gpos = {gx, gy};
// jfx::Array pos = jfx::Inverse(gpos);
obj.Lat = lat;
obj.Long = lon;
#else
double gx = 0.0f;
double gy = 0.0f;
CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
......@@ -476,6 +547,7 @@ void TrackingRos::ThreadTrackingProcess()
jfx::Array pos = jfx::Inverse(gpos);
obj.Lat = pos[0];
obj.Long = pos[1];
#endif
//SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f",obj.Lat,obj.Long);
}
if(iter.second->m_obj)
......@@ -524,6 +596,17 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
#ifdef _ABUZHABI_
double lon = 0.0f;
double lat = 0.0f;
// CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat);
// SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
// jfx::Array gpos = {gx, gy};
// jfx::Array pos = jfx::Inverse(gpos);
obj.Lat = lat;
obj.Long = lon;
#else
double gx = 0.0f;
double gy = 0.0f;
CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
......@@ -532,6 +615,7 @@ void TrackingRos::ThreadTrackingProcess()
jfx::Array pos = jfx::Inverse(gpos);
obj.Lat = pos[0];
obj.Long = pos[1];
#endif
//SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
}
}
......@@ -582,7 +666,6 @@ void TrackingRos::ThreadTrackingProcess()
box.pose.orientation.z = R_quat.z();
box.pose.orientation.w = R_quat.w();
sendBoxes.boxes.emplace_back(box);
obj.device = m_nodeName;
sendObjs.array.emplace_back(obj);
#ifdef _SEND_ABUZHABI_
Targets target = {};
......@@ -658,10 +741,10 @@ void TrackingRos::ThreadTrackingProcess()
t_marker.scale.x = 1.5;
t_marker.scale.y = 1.5;
t_marker.scale.z = 1.5;
t_marker.color.a = 1;
t_marker.color.r = 1.0;
t_marker.color.g = 0.0;
t_marker.color.b = 1.0;
t_marker.color.a = 1.0;
t_marker.color.r = 0;
t_marker.color.g = 1.0;
t_marker.color.b = 0;
t_marker.lifetime = ros::Duration(0.5);
char str[512] = {};
//sprintf(str, "id:{%llu},type:{%d},type_name:{%s},v:{%.2f}m/s,No:{%s}\ncolor_name:{%s},timestamp:{%llu}",
......@@ -688,7 +771,7 @@ void TrackingRos::ThreadTrackingProcess()
#ifdef _SEND_ABUZHABI_
static int sendNum = 0;
sendNum++;
if(sendNum%3 == 0)
if (sendNum % 3 == 0)
{
std::stringstream ss;
jf_ajson::save_to(ss, send_abu);
......
TrackingRos.h
View file @ 6c390320
......@@ -52,12 +52,16 @@ public:
std::vector<std::vector<double>> m_trans;
std::vector<std::vector<double>> m_kitti2origin;
#ifdef _ABUZHABI_
std::vector<double> m_UTM_station;
std::vector<double> m_wgs84_station;
std::vector<std::vector<double>> m_trans2station;
#endif
int m_frameNum = 0;
float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
std::string m_root_dir;//存储统一的根目录路径
std::string m_nodeName;//点位信息，现在就是10-1，1-1等信息
YAML::Node m_config;//配置的yaml文件
};
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