修改绝对位置的跟踪逻辑

TrackEx/Rel2AbsLoc.cpp

@@ -63,11 +63,11 @@ int Rel2AbsLoc::SetLocalization(double lon, double lat, double alt,double roll,
     m_trans.LatLon2Map(lat_, lon_0, northing_0, easting_0, m_zone);
     m_trans.LatLon2Map(lat_, lon_1, northing_1, easting_1, m_zone);
     double bias_angle_z = atan2(northing_1 - northing_0, easting_1 - easting_0);
-    m_yaw_bias = yaw_arc + bias_angle_z; 		//由于utm地图的变形，修正定位提供的航向角
+    m_yaw_bias = -yaw_arc + bias_angle_z; 		//由于utm地图的变形，修正定位提供的航向角
 
     // 2.3 cpt转enu外参
     m_imu_enu_extrinsic = Eigen::Isometry3d::Identity();
-    m_imu_enu_extrinsic.linear() = (Eigen::AngleAxisd(-m_yaw_bias, Eigen::Vector3d::UnitZ()) * //  pitch roll由定位提供
+    m_imu_enu_extrinsic.linear() = (Eigen::AngleAxisd(m_yaw_bias + M_PI, Eigen::Vector3d::UnitZ()) * //  pitch roll由定位提供
                                                 Eigen::AngleAxisd(-pitch_arc, Eigen::Vector3d::UnitY()) *
                                                 Eigen::AngleAxisd(roll_arc, Eigen::Vector3d::UnitX())).toRotationMatrix();
     return 0;
@@ -89,7 +89,7 @@ int Rel2AbsLoc::Transform(float x,float y, float z, float rot_y,float& utmX,floa
     utm_yaw = m_yaw_bias + rot_y;
 
     m_trans.Map2LatLon(utmY,utmX,m_zone,lat,lon);
-    
+
     return 0;
 }
 

TrackingRosAbs.cpp

@@ -359,7 +359,7 @@ void TrackingRos::Init(ros::NodeHandle& nh)
         if(config["save_log_file"])
             logFile = config["save_log_file"].as<std::string>();
         ADD_DEF_LOG(1, logFile.c_str());
-        SDK_IDX_LOG(1, SDK_INFO, "cur_time,frame,frameId,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,long,lat,input_x,input_y,input_z,intput_rot");
+        // SDK_IDX_LOG(1, SDK_INFO, "cur_time,frame,frameId,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,long,lat,input_x,input_y,input_z,intput_rot");
 // frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name
 // 时间戳（整型）,目标类型（整型）,置信度（浮点）,高度（浮点）,宽度（浮点）,长度（浮点）,中心点局部x坐标（浮点）,中心点局部y坐标（浮点）,中心点局部z坐标（浮点）,航向角（浮点）,跟踪ID（整型）,速度x（浮点）,速度y（浮点）,速度z（浮点）,定位点x（浮点）,定位点y（浮点）,定位点z（浮点）,定位点纬度（双精度浮点）,定位点经度（双精度浮点）,中心点纬度（双精度浮点）,中心点经度（双精度浮点）,名称（字符串,可选）,车牌号（字符串,可选）,颜色（字符串,可选）        
     }
@@ -688,10 +688,13 @@ void TrackingRos::ThreadTrackingProcessEx()
                     // }
 // frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name
 // 当前系统时间戳（整形）,时间戳（整型）,目标类型（整型）,置信度（浮点）,高度（浮点）,宽度（浮点）,长度（浮点）,中心点局部x坐标（浮点）,中心点局部y坐标（浮点）,中心点局部z坐标（浮点）,航向角（浮点）,跟踪ID（整型）,速度x（浮点）,速度y（浮点）,速度z（浮点）,定位点x（浮点）,定位点y（浮点）,定位点z（浮点）,定位点纬度（双精度浮点）,定位点经度（双精度浮点）,中心点纬度（双精度浮点）,中心点经度（双精度浮点）,名称（字符串,可选）,车牌号（字符串,可选）,颜色（字符串,可选）
-                    SDK_IDX_LOG(1, SDK_INFO, "%llu,%llu,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%u,%f,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%f,%f,%f",
-                        cur_systime_rd,obj.frame,m_frameNum,obj.type,obj.score,obj.h,obj.w,obj.l,obj.x,obj.y,obj.z,obj.rot_y,
-                        obj.obj_id,obj.v_x,obj.v_y,obj.v_z,obj.loc_x,obj.loc_y,obj.loc_z,obj.Long,obj.Lat,
-                        objsPtr->array[updateId[iter.first]].x,objsPtr->array[updateId[iter.first]].y,objsPtr->array[updateId[iter.first]].z,objsPtr->array[updateId[iter.first]].rot_y);
+                    // SDK_IDX_LOG(1, SDK_INFO, "%llu,%llu,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%u,%f,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%f,%f,%f",
+                    //     cur_systime_rd,obj.frame,m_frameNum,obj.type,obj.score,obj.h,obj.w,obj.l,obj.x,obj.y,obj.z,obj.rot_y,
+                    //     obj.obj_id,obj.v_x,obj.v_y,obj.v_z,obj.loc_x,obj.loc_y,obj.loc_z,obj.Long,obj.Lat,
+                    //     objsPtr->array[updateId[iter.first]].x,objsPtr->array[updateId[iter.first]].y,objsPtr->array[updateId[iter.first]].z,objsPtr->array[updateId[iter.first]].rot_y);
+                    if(obj.type != 10)
+                        SDK_IDX_LOG(1, SDK_INFO, "%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%d,%s",
+                            obj.obj_id,m_frameNum,obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,rot_y_angle,obj.score,obj.type,obj.name.c_str());
 
                     static float UTMX = 0;
                     static float UTMY = 0;
@@ -948,6 +951,7 @@ void TrackingRos::ThreadAbsLocProcess()
             selfInfo.rot_y = (locPtr->Yaw)/180*M_PI; 
             selfInfo.Long = locPtr->lon;
             selfInfo.Lat = locPtr->lat;
+            selfInfo.name = "self";
             objsPtr->array.emplace_back(selfInfo);          
             m_objsQueue.push(objsPtr);          
         }