Skip to content

J
jfx_rvfusion_cx
Commit 8db78272 authored by alexander's avatar alexander
Browse files
Options

融合数据代码更新

parent 669f7a44
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 394 additions and 148 deletions
CMakeLists.txt
View file @ 8db78272
......@@ -5,7 +5,7 @@ project(jfx_rvfusion_cx VERSION 1.0)
## compiler ##
##############
set(CMAKE_BUILD_TYPE "Debug")
#set(CMAKE_BUILD_TYPE "Debug")
add_compile_options(-std=c++14)
......@@ -18,6 +18,9 @@ ENDIF()
message("${PROJECT_NAME} Build type: ${CMAKE_BUILD_TYPE}")
#attention!!!
add_definitions(-D_DEBUG)
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
add_definitions(-D_DEBUG)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0")
......
incl/fusionAlgo.h
View file @ 8db78272
......@@ -35,6 +35,8 @@ public:
static void fitFObjNextPoint(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj& obj);
static void fitFObjHeading(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj& obj);
static double PointsAngle(double sdLon,double sdLat,double edLon, double edLat) {
try {
double azi1 = 0;
......@@ -49,4 +51,6 @@ public:
}
static bool intersection( const STLine &l1, const STLine &l2);
static int checkInArea(std::vector<std::vector<double>> &poly, const std::vector<double> &pt);
};
\ No newline at end of file
incl/fusionConfig.h
View file @ 8db78272
......@@ -18,6 +18,7 @@ public:
int init(string yamlFile)
{
try {
std::cout<<yamlFile<<std::endl;
YAML::Node conf = YAML::LoadFile(yamlFile);
mStart = conf["GlobalConfig"]["start"].as<bool>();
mMecID = conf["GlobalConfig"]["mec_id"].as<string>();
......@@ -82,8 +83,15 @@ public:
mFilterDevID[n] = enTSModeVideo;
}
}
m_lidar_delay = conf["FusionConfig"]["lidar_delay"].as<int32_t>();
m_radar_delay = conf["FusionConfig"]["radar_delay"].as<int32_t>();
m_vision_delay = conf["FusionConfig"]["vision_delay"].as<int32_t>();
m_fusion_delay = conf["FusionConfig"]["fusion_delay"].as<int32_t>();
mIfDynamicChangeDropFrameStep = conf["FusionConfig"]["dynamicDropFrame"].as<bool>();
m_check_roi = conf["FusionConfig"]["checkRoi"].as<bool>();
m_correct_type = conf["FusionConfig"]["dynamicCorrectType"].as<bool>();
m_fusion_correct_type = conf["FusionConfig"]["fusionCorrectType"].as<bool>();
// heading keep
auto itrHeadingKeep = conf["FusionConfig"]["heading_keep"].begin();
for (; itrHeadingKeep != conf["FusionConfig"]["heading_keep"].end(); ++itrHeadingKeep) {
......@@ -93,24 +101,28 @@ public:
}
setHeadingKeepArea(vecHK);
}
//filter_roi
std::vector<std::vector<double>> filter_roi = conf["FusionConfig"]["filter_roi"].as<std::vector<std::vector<double>>>();
for(size_t i=0; i+2 < filter_roi.size(); i=i+2)
//heading area
std::vector<std::vector<double>> head_area = conf["FusionConfig"]["heading_area"].as<std::vector<std::vector<double>>>();
std::vector<std::vector<double>> head_locat;
for(size_t i=0; i<head_area.size(); i++)
{
STLine line;
line.x1 = filter_roi[i][0];
line.y1 = filter_roi[i][1];
line.x2 = filter_roi[i+1][0];
line.y2 = filter_roi[i+1][1];
mFilterLine.push_back(line);
if(head_area[i][1]<0)
{
m_heading_area[head_area[i][0]] = head_locat;
head_locat.clear();
}
else
{
head_locat.push_back(head_area[i]);
}
}
for(auto itr = m_heading_area.begin();itr != m_heading_area.end();itr++)
{
std::cout<<"heading:"<<itr->first<<",value:"<<itr->second.size()<<std::endl;
}
STLine line;
size_t pos = filter_roi.size();
line.x1 = filter_roi[pos-1][0];
line.y1 = filter_roi[pos-1][1];
line.x2 = filter_roi[0][0];
line.y2 = filter_roi[0][1];
mFilterLine.push_back(line);
//filter_roi
m_roi_area = conf["FusionConfig"]["filter_roi"].as<std::vector<std::vector<double>>>();
if (conf["FusionConfig"]["KITTI2ORIGIN"])
{
m_kitti2origin = conf["FusionConfig"]["KITTI2ORIGIN"].as<std::vector<std::vector<double>>>();
......@@ -135,7 +147,10 @@ public:
m_check_in_map = conf["FusionConfig"]["checkInMap"].as<bool>();
m_pro_dir = conf["FusionConfig"]["pro_dir"].as<std::string>();
m_map_cfg = conf["FusionConfig"]["map_cfg"].as<std::string>();
m_root_dir = conf["FusionConfig"]["root_dir"].as<std::string>();
m_lidar_inner_area = conf["FusionConfig"]["lidar_inner_area"].as<std::vector<std::vector<double>>>();
m_radar_outer_area = conf["FusionConfig"]["radar_outer_area"].as<std::vector<std::vector<double>>>();
m_fusion_type_area = conf["FusionConfig"]["fusion_type_area"].as<std::vector<std::vector<double>>>();
} catch (const std::exception& e) {
ROS_WARN("load fusion config fail: %s", yamlFile.c_str());
std::cerr << e.what() << '\n';
......@@ -239,8 +254,15 @@ public:
mFilterDevID[n] = enTSModeVideo;
}
}
m_lidar_delay = conf["FusionConfig"]["lidar_delay"].as<int32_t>();
m_radar_delay = conf["FusionConfig"]["radar_delay"].as<int32_t>();
m_vision_delay = conf["FusionConfig"]["vision_delay"].as<int32_t>();
m_fusion_delay = conf["FusionConfig"]["fusion_delay"].as<int32_t>();
mIfDynamicChangeDropFrameStep = conf["FusionConfig"]["dynamicDropFrame"].as<bool>();
m_check_roi = conf["FusionConfig"]["checkRoi"].as<bool>();
m_correct_type = conf["FusionConfig"]["dynamicCorrectType"].as<bool>();
m_fusion_correct_type = conf["FusionConfig"]["fusionCorrectType"].as<bool>();
// heading keep
auto itrHeadingKeep = conf["FusionConfig"]["heading_keep"].begin();
for (; itrHeadingKeep != conf["FusionConfig"]["heading_keep"].end(); ++itrHeadingKeep) {
......@@ -250,25 +272,28 @@ public:
}
setHeadingKeepArea(vecHK);
}
//filter_roi
std::vector<std::vector<double>> filter_roi = conf["FusionConfig"]["filter_roi"].as<std::vector<std::vector<double>>>();
for(size_t i=0; i+2 < filter_roi.size(); i=i+2)
//heading area
std::vector<std::vector<double>> head_area = conf["FusionConfig"]["heading_area"].as<std::vector<std::vector<double>>>();
std::vector<std::vector<double>> head_locat;
for(size_t i=0; i<head_area.size(); i++)
{
STLine line;
line.x1 = filter_roi[i][0];
line.y1 = filter_roi[i][1];
line.x2 = filter_roi[i+1][0];
line.y2 = filter_roi[i+1][1];
mFilterLine.push_back(line);
if(head_area[i][1]<0)
{
m_heading_area[head_area[i][0]] = head_locat;
head_locat.clear();
}
else
{
head_locat.push_back(head_area[i]);
}
}
STLine line;
size_t pos = filter_roi.size();
line.x1 = filter_roi[pos-1][0];
line.y1 = filter_roi[pos-1][1];
line.x2 = filter_roi[0][0];
line.y2 = filter_roi[0][1];
mFilterLine.push_back(line);
for(auto itr = m_heading_area.begin();itr != m_heading_area.end(); itr++)
{
std::cout<<"heading:"<<itr->first<<",value:"<<itr->second.size()<<std::endl;
}
//filter_roi
m_roi_area = conf["FusionConfig"]["filter_roi"].as<std::vector<std::vector<double>>>();
if (conf["KITTI2ORIGIN"])
{
m_kitti2origin = conf["KITTI2ORIGIN"].as<std::vector<std::vector<double>>>();
......@@ -293,7 +318,9 @@ public:
m_check_in_map = conf["FusionConfig"]["checkInMap"].as<bool>();
m_pro_dir = conf["pro_dir"].as<std::string>();
m_map_cfg = conf["map_cfg"].as<std::string>();
m_lidar_inner_area = conf["FusionConfig"]["lidar_inner_area"].as<std::vector<std::vector<double>>>();
m_radar_outer_area = conf["FusionConfig"]["radar_outer_area"].as<std::vector<std::vector<double>>>();
m_fusion_type_area = conf["FusionConfig"]["fusion_type_area"].as<std::vector<std::vector<double>>>();
} catch (const std::exception& e) {
ROS_WARN("load fusion config failed");
std::cerr << e.what() << '\n';
......@@ -365,7 +392,8 @@ public:
ENTrafficSensMode mFirstOutMode = enTSModeVideo;
string mCreateFusionObjMode = "lr";
int mObjRetainMS = 5000;
//int mObjRetainMS = 5000;
int mObjRetainMS = 360000;
int mFilterMaxRadius = 250;
int mFusionRadius = 250;
......@@ -389,6 +417,7 @@ public:
double mDistThresholdVehicleSameMode = 1;
int mTrajNearMinPoint = 4;
//int mObjFusionMinPoint = 10;
int mObjFusionMinPoint = 10;
int mObjFusionMaxPoint = 500;
......@@ -423,6 +452,18 @@ public:
string m_pro_dir;
string m_map_cfg;
string m_root_dir;
bool m_check_roi = false;
bool m_correct_type = false;
bool m_fusion_correct_type = false;
int32_t m_lidar_delay = 0;
int32_t m_radar_delay = 0;
int32_t m_vision_delay = 0;
int32_t m_fusion_delay = 0;
std::vector<std::vector<double>> m_roi_area;
std::vector<std::vector<double>> m_lidar_inner_area;
std::vector<std::vector<double>> m_radar_outer_area;
std::vector<std::vector<double>> m_fusion_type_area;
std::map<double,std::vector<std::vector<double>>> m_heading_area;
};
#define FConfPtr (FusionConfig::Instance())
incl/fusionDataLog.h
View file @ 8db78272
......@@ -58,9 +58,9 @@ protected:
int saveOneTObj(TOBJPtr& tObjPtr)
{
string buf(2048, 0);
sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,0,%.2lf,%.2lf,%s,%ld\n",
sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,%.2lf,%.2lf,%.2lf,%s,%ld\n",
tObjPtr->mMS, tObjPtr->mID, tObjPtr->mTypeStr.c_str(), tObjPtr->mHeading, tObjPtr->mLon, tObjPtr->mLat,
tObjPtr->mDevIDStr.c_str(), tObjPtr->mLength, tObjPtr->mWidth, tObjPtr->mModeStr.c_str(),
tObjPtr->mDevIDStr.c_str(), tObjPtr->mHeight,tObjPtr->mLength, tObjPtr->mWidth, tObjPtr->mModeStr.c_str(),
mills_UTC() - tObjPtr->mMS);
saveData(buf.c_str());
......@@ -70,9 +70,10 @@ protected:
int saveOneFObj(jfx_common_msgs::OutFusionObj& fobj, FOBJPtr& fObjPtr)
{
string buf(2048, 0);
sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,%.2lf,%.2lf,%.2lf,%ld\n",
sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,%.2lf,%.2lf,%.2lf,%ld,%d,%d,%d\n",
fobj.time, fobj.all_id, fobj.type.c_str(), fobj.heading, fobj.lon, fobj.lat,
fobj.inner_pos.c_str(), fobj.obj_l, fobj.obj_w, fobj.speed, fObjPtr->mMainObj->mID);
fobj.inner_pos.c_str(), fobj.obj_l, fobj.obj_w, fobj.speed, fObjPtr->mMainObj->mID,
fObjPtr->mMainObj->mCheckNearPos,fObjPtr->mMainObj->mOutPos,mills_UTC()-fobj.time);
saveData(buf.c_str());
return 0;
......
incl/fusionDef.h
View file @ 8db78272
......@@ -22,7 +22,8 @@ typedef enum _tagENTrafficObjectType {
enTOTypeMotorbike = 3,
enTOTypeCar = 4,
enTOTypeTruck = 5,
enTOTypeBus = 6
enTOTypeBus = 6,
enTOTypeTricycle = 7
} ENTrafficObjectType;
typedef struct _tagSTObjTrajPoint {
......@@ -34,6 +35,7 @@ typedef struct _tagSTObjTrajPoint {
bool isUseHeadingKeep = false;
double speed = 0;
int laneseqnum = 0;
ENTrafficSensMode mode= enTSModeUnknown;
_tagSTObjTrajPoint() { }
_tagSTObjTrajPoint(uint64_t _ms, double _lon, double _lat,
double _heading, double _headingReal, double _speed, int _lane)
......
incl/fusionDistribTask.h
View file @ 8db78272
......@@ -36,6 +36,7 @@ public:
public:
int init(ros::NodeHandle& nh, string replayCsv = "");
void init();
void onRadarData(const jfx_common_msgs::RadarData& data);
void onVisionData(const jfx_common_msgs::det_tracking_array& data);
......@@ -48,15 +49,18 @@ public:
bool ifDropFrame(SDEVPtr& sensDevPtr, int checkNearQueueLen);
bool ifDropFrameTime(std::shared_ptr<TrafficObject>& tObjPtr);
bool ifCorrectType(std::shared_ptr<TrafficObject>& tObjPtr);
bool filter(std::shared_ptr<TrafficObject>& tObjPtr);
bool filterRoi(std::shared_ptr<TrafficObject>& tObjPtr);
bool filterArea(std::shared_ptr<TrafficObject>& tObjPtr);
bool filterInMap(std::shared_ptr<TrafficObject>& tObjPtr);
int IsInMap(float x, float y, float z, float rot_y, double& lon, double& lat,double& laneAngle);
int IsInMapV2(float rot_y, double lon, double lat,double& laneAngle);
void pubState();
......@@ -83,4 +87,4 @@ public:
};
#define FDistribTaskPtr (FusionDistributeTask::Instance())
\ No newline at end of file
#define FDistribTaskPtr (FusionDistributeTask::Instance())
incl/fusionObject.h
View file @ 8db78272
......@@ -25,7 +25,9 @@ public:
void setInValid();
void setHisPoint(jfx_common_msgs::OutFusionObj& obj);
void setHisPoint(jfx_common_msgs::OutFusionObj& obj, ENTrafficSensMode mode);
void InitDefaultTypeStr();
std::string getTrueTypeStr();
......@@ -34,6 +36,7 @@ public:
public:
uint64_t mID = 0;
uint64_t mMS = 0;
ENTrafficSensMode mMode = enTSModeUnknown;
ENTrafficObjectType mType = enTOTypeUnknown;
double mLon = 0;
......@@ -44,7 +47,7 @@ public:
TOBJPtr mMainObj = nullptr;
uint64_t mLastOutMS = 0;
uint64_t mLastOutMainObjMS = 0;
uint64_t mLastOutFusionMs = 0;
std::mutex mTObjMapMutex;
std::map<uint64_t, TOBJPtr> mRTObjMap;
std::map<uint64_t, TOBJPtr> mLTObjMap;
......@@ -55,9 +58,9 @@ public:
double mLastCalcuHeadingLon = 0;
double mLastCalcuHeadingLat = 0;
std::string mDefaultLidarType = "";
std::string mDefaultRadarType = "";
std::string mDefaultVideoType = "";
std::string mDefaultType = "";
bool mIsCheck=false;
int mCount = 0;
};
#define FOBJPtr std::shared_ptr<FusionObject>
incl/sensDevice.h
View file @ 8db78272
......@@ -24,6 +24,9 @@ public:
uint64_t mLastDataLocalMS = 0;
uint64_t mLLastDataMS = 0;
uint64_t mMaxMs = 0;//设备当前最大的时间
uint64_t mArriveLastMs = 0;//到达MEC的最新时间
uint64_t mArriveLLastMs = 0;//到达MEC的最新的上一帧的时间
};
#define SDEVPtr std::shared_ptr<SensorDevice>
\ No newline at end of file
incl/trafficObject.h
View file @ 8db78272
......@@ -9,6 +9,7 @@
#include <mutex>
#include "fusionDef.h"
#include "utils.h"
using namespace std;
//轨迹点，其实也是交通目标
......@@ -39,6 +40,8 @@ public:
bool isFarObj(std::shared_ptr<TrafficObject>& tObjPtr);
bool isNearObj(std::shared_ptr<TrafficObject>& tObjPtr);
void removeNearObj(uint64_t key);
/// Reset max near score
......@@ -100,8 +103,6 @@ public:
std::mutex mFarObjMapMutex;
std::unordered_map<uint64_t, STFarObj> mFarObjMap;
std::unordered_map<ENTrafficObjectType, uint64_t> mTypeCountMap;
// 历史数据，理论上需要锁定，但不需要锁定
std::vector<STObjTrajPoint> mHisDataReal;
std::vector<STObjTrajPoint> mHisData;
......@@ -109,6 +110,11 @@ public:
bool mIsUseHeadingKeep = false;
int mCheckNearPos = 0;
int mOutPos = 0;
double mLaneAngle = 0;
bool mIsCheckType = false;
std::unordered_map<ENTrafficObjectType, int> mTypeCountMap;
uint64_t mArriveMS = 0;//到达MEC的时间，这里主要记录帧
bool isMatch = false;
};
#define TOBJPtr std::shared_ptr<TrafficObject>
......
src/fusionAlgo.cpp
View file @ 8db78272
......@@ -37,16 +37,19 @@ double FusionAlgo::calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t
pos = 0;
}
// ToDO: 反过来也要处理
// ToDO:
//找最近的点的前提是两个点的时间在相同的情况下位置接近
//后退的前提是差多少时间两个点位置比较接近
uint64_t backOffMS = 0;
if (obj1->mMode == enTSModeVideo && obj2->mMode == enTSModeRadar) {
/*
if (obj1->mMode == enTSModeLidar && obj2->mMode == enTSModeRadar) {
backOffMS = 300;
}
//毫米波比激光要出现的早，所有毫米波后退300ms？？
if (obj1->mMode == enTSModeRadar && obj2->mMode == enTSModeLidar) {
backOffMS = 300;
}
*/
// Calculate p1,p2,p3 of object1
STObjTrajPoint p1 = obj1->mHisData[pos];
double offset1 = obj1->mHeading == 0 ? 0 : obj1->mLength / 2 + p1.speed / 3;
......@@ -114,10 +117,12 @@ double FusionAlgo::calcuObjCurDist2(int pos, TOBJPtr obj1, TOBJPtr obj2) {
int FusionAlgo::ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o)
{
// 空指针判断
if (!tObjPtr || !o) {
return -1;
}
// 目标丢失或者无效
if (o->mBLost || o->mValid == false) {
return -2;
}
......@@ -127,6 +132,16 @@ int FusionAlgo::ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o)
return -3;
}
//应该只能是毫米波和激光雷达之间融合
//激光雷达是拼接的，相同类型的，不用融合，减少计算量
if( tObjPtr->mMode == enTSModeLidar && o->mMode == enTSModeLidar){
return -7;
}
//毫米波虽然是多个，但是每个方向一个，每个方向目标是独立的，不应该检查，减少计算量
if( tObjPtr->mMode == enTSModeRadar && o->mMode == enTSModeRadar){
return -8;
}
if (FConfPtr->mCreateFusionObjMode.find(o->mModeStr) == std::string::npos) {
return -1;
}
......@@ -144,27 +159,36 @@ int FusionAlgo::ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o)
return false;
}
*/
//如果o是毫米波并且已经匹配了，就不能再匹配了
if(o->mMode == enTSModeRadar ){
if(tObjPtr->isNearObj(o)){
//表示tObjPtr 和o 已经绑定
}else if(o->isMatch == true){
return -9;
}
}
//只能跟新目标融合，不可能跟旧目标融合，旧目标应该是生成了，旧目标应该是已经有融合id了
//为什么产生融合目标的时候，如果fObject->mFID为空，直接返回
// 当前目标比其他目标出现的早，就不判断了，就是说早出现的目标应该是已经被处理过了？
if (tObjPtr->mIdx < o->mIdx) {
return -4;
}
//是否检查类型，目前不检查类型，毫米波雷达类型不准，从远处过来的，可能是摩托车，显示为car
// 是否检查类型，目前不检查类型，毫米波雷达类型不准，从远处过来的，可能是摩托车，显示为car
if (FConfPtr->mIfCheckObjType && tObjPtr->isSameType(o) == false) {
return -5;
}
// 在之前的CheckNear中，发下o离的很远，没必要继续CheckNear
if (tObjPtr->isFarObj(o)) {
return -6;
}
/*航向不检查了
//是否检查目标航向
/*
if (FConfPtr->mIfCheckObjHeading && jfx::GetHeadingChange(tObjPtr->mHeading, o->mHeading) > 90) {
return false;
}*/
}
*/
return 0;
}
......@@ -203,8 +227,14 @@ void FusionAlgo::updateNearInfo(TOBJPtr& tObjPtr, TOBJPtr& o, double dist, uint6
nearObj.id = o->mID;
tObjPtr->mNearObjMap[nearObj.key] = nearObj;
itrNear = tObjPtr->mNearObjMap.find(o->mKey);
//如果o是毫米波，标记已经被匹配了
//一个激光雷达可以匹配多个毫米波，但是一个毫米波只能匹配一个激光雷达：原因是激光雷达在路中心，毫米波在路远端
if(o->mMode == enTSModeRadar) {
o->isMatch = true;
}
}
//防止重复计算
if (itrNear->second.lastMS == oms) {
return;
}
......@@ -215,6 +245,7 @@ void FusionAlgo::updateNearInfo(TOBJPtr& tObjPtr, TOBJPtr& o, double dist, uint6
itrNear->second.totalDMS += dms;
itrNear->second.lastLon = o->mLon;
itrNear->second.lastLat = o->mLat;
itrNear->second.lastMS = oms;
if (itrNear->second.count >= FConfPtr->mTrajNearMinPoint) {
if (itrNear->second.totalDist > 0 && itrNear->second.count > 0) {
......@@ -228,7 +259,7 @@ void FusionAlgo::updateNearInfo(TOBJPtr& tObjPtr, TOBJPtr& o, double dist, uint6
tObjPtr->mMaxNearKey = itrNear->second.key;
}
//
//重复执行了，不过也没关系，没啥问题
itrNear = tObjPtr->mNearObjMap.find(tObjPtr->mMaxNearKey);
if (itrNear != tObjPtr->mNearObjMap.end()) {
tObjPtr->mMaxNearScore = itrNear->second.score;
......@@ -241,8 +272,16 @@ void FusionAlgo::fitFObjNextPoint(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj&
if (fObj->mFusionHisData.size() < 1 || fObj->mMainObj == nullptr) {
return;
}
int pos = fObj->mFusionHisData.size() > 4 ? fObj->mFusionHisData.size() - 4 : 0;
//在停车或者低速的情况下保持位置不变，防止穿模
//将上一个位置覆盖到当前的位置，将原先的值覆盖掉
/*
if(obj.speed < 1.9 && fObj->mMainObj->mTypeStr != "Person"){
int lastpos = fObj->mFusionHisData.size() - 1;
obj.lon = fObj->mFusionHisData[lastpos].lon;
obj.lat = fObj->mFusionHisData[lastpos].lat;
}
*/
int pos = fObj->mFusionHisData.size() > 2 ? fObj->mFusionHisData.size() - 2 : 0;
double lon = obj.lon, lat = obj.lat;
int count = 1;
for (int i = pos; i < (int)fObj->mFusionHisData.size(); i++) {
......@@ -252,52 +291,29 @@ void FusionAlgo::fitFObjNextPoint(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj&
count++;
}
}
obj.lon = lon / count;
obj.lat = lat / count;
/*
// 轨迹倒退或横向调整大的问题
auto p1 = fObj->mFusionHisData[0];
auto p4 = fObj->mFusionHisData[3];
double headingHis = jfx::GetDirectionXS(p1.lon, p1.lat, p4.lon, p4.lat);
double headingNow = jfx::GetDirectionXS(p4.lon, p4.lat, obj.lon, obj.lat);
double headingChange1 = jfx::GetHeadingChange(headingHis, headingNow);
if (headingHis != 0 && abs(headingChange1) > 90) {
obj.lon = p4.lon;
obj.lat = p4.lat;
return;
//计算更新
{
obj.lon = lon / count;
obj.lat = lat / count;
}
}
auto lastPoint = fObj->mFusionHisData[fObj->mFusionHisData.size() - 1];
double headingChange = abs(jfx::GetHeadingChange(lastPoint.heading, obj.heading));
if (headingChange > 30) {
void FusionAlgo::fitFObjHeading(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj& obj)
{
//小于两个节点不用计算了。
if (fObj->mFusionHisData.size() < 2 || fObj->mMainObj == nullptr) {
return;
}
double totalDist = 0;
double count = 0;
double totalMS = obj.time - fObj->mFusionHisData.begin()->ms;
for (int i = 0; i < (fObj->mFusionHisData.size() - 1); i++) {
totalDist += jfx::GetDistance(fObj->mFusionHisData[i].lon, fObj->mFusionHisData[i].lat,
fObj->mFusionHisData[i + 1].lon, fObj->mFusionHisData[i + 1].lat);
count++;
if(fObj->mDefaultType == "Person"){
int start_pos = fObj->mFusionHisData.size() > 5 ? fObj->mFusionHisData.size() - 5 : 0;
int end_pos = fObj->mFusionHisData.size() - 1;
STObjTrajPoint& startPoint = fObj->mFusionHisData[start_pos];
STObjTrajPoint& endPoint = fObj->mFusionHisData[end_pos];
double heading = FusionAlgo::geoGetHeading(startPoint.lon, startPoint.lat, endPoint.lon, endPoint.lat);
obj.heading = heading;
}
double lastStep = jfx::GetDistance(lastPoint.lon, lastPoint.lat, obj.lon, obj.lat);
totalDist += lastStep;
count++;
double avgStep = totalDist / count;
if (lastStep <= (avgStep * 1.1) || avgStep == 0) {
return;
}
double lastHeading = jfx::GetDirectionXS(lastPoint.lon, lastPoint.lat, obj.lon, obj.lat) + 1.26;
lastHeading = lastHeading > 360 ? (lastHeading - 360) : lastHeading;
jfx::Array toPoint = jfx::GetPoint(lastPoint.lon, lastPoint.lat, avgStep, lastHeading);
obj.lon = toPoint[0];
obj.lat = toPoint[1];*/
}
bool FusionAlgo::intersection(const STLine &l1, const STLine &l2)
......@@ -319,4 +335,15 @@ bool FusionAlgo::intersection(const STLine &l1, const STLine &l2)
return false;
}
return true;
}
\ No newline at end of file
}
int FusionAlgo::checkInArea(std::vector<std::vector<double>> &poly, const std::vector<double> &pt) //返回1就是在多边形里，0是在多边形外
{
size_t i, j, c = 0;
for (i = 0, j = poly.size() - 1; i < poly.size(); j = i++) {
if (((poly[i][1] > pt[1]) != (poly[j][1] > pt[1])) &&
(pt[0] < (poly[j][0] - poly[i][0]) * (pt[1] - poly[i][1]) / (poly[j][1] - poly[i][1]) + poly[i][0]))
c = !c;
}
return c;
}
src/fusionCheckNearTask.cpp
View file @ 8db78272
......@@ -72,7 +72,7 @@ void FusionCheckNearTask::run()
if (mNextTask) {
mNextTask->pushTrafficObj(item);
} else if (item.pos >= FConfPtr->mObjFusionMinPoint) {//目标大于10点才开始融合
} else if (item.pos >= FConfPtr->mObjFusionMinPoint) {//目标大于10个点才开始融合
item.tObjPtr->mCheckNearPos = item.pos;//记录一下开始融合的点位置
FTrajTaskPtr->pushTrafficObj(item);
}
......@@ -82,7 +82,7 @@ void FusionCheckNearTask::run()
int FusionCheckNearTask::checkNear(STCheckNearItem& item)
{
auto tObjPtr = item.tObjPtr;
if (!tObjPtr) {
return -1;
}
......@@ -119,12 +119,17 @@ int FusionCheckNearTask::checkNear(STCheckNearItem& item)
continue;
} else {
if (itr->second->mFID == 0) {
//目标之前未融合，但是过了很久才出现
if (mLastCheckDataMS > itr->second->mMS && (mLastCheckDataMS - itr->second->mMS) > 3 * FConfPtr->mObjRetainMS) {
itr->second->mValid = false;
ROS_WARN("mFid =0 exception obj id: %s,%llu, %llu, %llu", itr->second->mModeStr.c_str(),itr->second->mID,
mLastCheckDataMS,itr->second->mMS);
}
} else {
if (mLastCheckDataMS > itr->second->mMS && (mLastCheckDataMS - itr->second->mMS) > 20 * FConfPtr->mObjRetainMS) {
itr->second->mValid = false;
ROS_WARN("mFid !=0 exception obj id: %s,%llu,%llu,%llu", itr->second->mModeStr.c_str(), itr->second->mID,
mLastCheckDataMS,itr->second->mMS);
}
}
checkTObjLost(itr->second);
......@@ -140,7 +145,7 @@ int FusionCheckNearTask::checkNear(STCheckNearItem& item)
if (FusionAlgo::ifCanCheckNear(tObjPtr, o) != 0) {
continue;
}
//oms:是o的时间，dms：是tObjPtr和o的时间差的绝对值
uint64_t oms = 0, dms = 0;
double dist = FusionAlgo::calcuObjCurDist(item.pos, tObjPtr, o, oms, dms);
if (FusionAlgo::isTwoObjNear(tObjPtr, o, dist) == false) {
......@@ -167,10 +172,29 @@ bool FusionCheckNearTask::checkTObjLost(TOBJPtr& tObjPtr)
return true;
}
if (devPtr->mLLastDataMS > tObjPtr->mMS && (tObjPtr->mCheckNearPos + 1) >= tObjPtr->mHisData.size()) {
//devPtr->mLLastDataMS:代表上上帧
//使用这个的前提是：每个传感器的每一帧里面的目标时间戳都一样
//这里有两种情况：
//1）已经进入融合的点：devPtr->mLLastDataMS > tObjPtr->mMS && (tObjPtr->mCheckNearPos + 1) >= tObjPtr->mHisData.size()
//2）没有进入融合的点：devPtr->mLLastDataMS > tObjPtr->mMS && (devPtr->mLLastDataMS - tObjPtr->mMS > 200)
if ( (devPtr->mLLastDataMS > tObjPtr->mMS && (tObjPtr->mCheckNearPos + 1) >= tObjPtr->mHisData.size()) ||
(devPtr->mLLastDataMS > tObjPtr->mMS && (devPtr->mLLastDataMS - tObjPtr->mMS) > 100) ){
ROS_WARN("Lost obj id: %s,%llu,%llu,%llu,%llu,%d,%d,%d", tObjPtr->mModeStr.c_str(), tObjPtr->mID,devPtr->mLastDataMS,devPtr->mLLastDataMS,
tObjPtr->mMS,tObjPtr->mCheckNearPos,tObjPtr->mHisData.size(),devPtr->mDataDelayMS);
tObjPtr->mBLost = true;
return true;
}
//目标丢失判断新方法
//这个有问题的原因是：进来的数据如果过滤了，但是用到达时间判断没有丢失。
/*
if (devPtr->mArriveLLastMs > tObjPtr->mArriveMS && (tObjPtr->mCheckNearPos + 1) >= tObjPtr->mHisData.size()) {
ROS_WARN("Lost obj id: %s,%llu,%llu,%llu,%d,%d,%d", tObjPtr->mModeStr.c_str(),tObjPtr->mID,devPtr->mArriveLLastMs,
tObjPtr->mArriveMS,tObjPtr->mCheckNearPos,tObjPtr->mHisData.size(),devPtr->mDataDelayMS);
tObjPtr->mBLost = true;
return true;
}
*/
return false;
}
src/fusionData.cpp
View file @ 8db78272
......@@ -24,7 +24,7 @@ TOBJPtr FusionData::setTrafficObj(TOBJPtr& tObjPtr)
if (itr == mTrafficObjMainMap.end()) {
tObjPtr->mKey = objKey;
tObjPtr->mFMS = tObjPtr->mMS;
tObjPtr->mIdx = mTfcObjIdx++;
tObjPtr->mIdx = mTfcObjIdx++;//mTfcObjIdx,第几个目标
mTrafficObjMainMap[objKey] = tObjPtr;
itr = mTrafficObjMainMap.find(objKey);
}
......@@ -34,6 +34,8 @@ TOBJPtr FusionData::setTrafficObj(TOBJPtr& tObjPtr)
}
// 更新目标属性
itr->second->mDevIDStr = tObjPtr->mDevIDStr;
itr->second->mModeStr = tObjPtr->mModeStr;
itr->second->mLon = tObjPtr->mLon;
itr->second->mLat = tObjPtr->mLat;
itr->second->mMS = tObjPtr->mMS;
......@@ -41,27 +43,69 @@ TOBJPtr FusionData::setTrafficObj(TOBJPtr& tObjPtr)
itr->second->mWidth = tObjPtr->mWidth;
itr->second->mHeight = tObjPtr->mHeight;
itr->second->mLaneSeqNum = tObjPtr->mLaneSeqNum;
itr->second->mType = tObjPtr->mType;
itr->second->mCount++;
itr->second->mArriveMS = tObjPtr->mArriveMS;//这个时间需要更新
itr->second->mCount++;//目标的点数
//itr->second->mType = tObjPtr->mType;
//itr->second->mTypeStr = tObjPtr->mTypeStr;
//固定类型，采用融合前的最后一个点的类型作为最终类型
if(itr->second->mIsCheckType == false){
if(itr->second->mCount >= FConfPtr->mObjFusionMinPoint){
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
itr->second->mIsCheckType = true;
}else{
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
}
}
//毫米波bus类型固定
/*
if(itr->second->mIsCheckType == false){
if(itr->second->mCount >= FConfPtr->mObjFusionMinPoint){
if(tObjPtr->mModeStr == "r"){
if(tObjPtr->mTypeStr == "Bus"){
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
itr->second->mIsCheckType = true;
}else{
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
}
}else{
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
itr->second->mIsCheckType = true;
}
}else{
if(tObjPtr->mTypeStr == "Bus"){
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
itr->second->mIsCheckType = true;
}else{
itr->second->mType = tObjPtr->mType;
itr->second->mTypeStr = tObjPtr->mTypeStr;
}
}
}
*/
itr->second->mFusionMS = mDataMSMax;
itr->second->mBLost = false;
itr->second->mValid = true;
double speedX = itr->second->genSpeedX();
//itr->second->mHeading = itr->second->genHeading();
itr->second->mHeading = itr->second->genHeading(speedX,10);
//itr->second->mHeading = itr->second->genHeading(speedX,10);
itr->second->mHeading = itr->second->genHeading(speedX,4);
itr->second->mSpeed = speedX;
itr->second->mDist2Center = tObjPtr->mDist2Center;
//为什么用3帧？？
//double headingReal = itr->second->genHeading(3);
double headingReal = itr->second->mHeading;
double headingReal = itr->second->genHeading(speedX,3);
// 更新目标历史轨迹
STObjTrajPoint hisData(itr->second->mMS, itr->second->mLon, itr->second->mLat,
itr->second->mHeading, headingReal, itr->second->mSpeed, itr->second->mLaneSeqNum);
itr->second->mHeading, headingReal, itr->second->mSpeed, itr->second->mLaneSeqNum);
hisData.isUseHeadingKeep = itr->second->mIsUseHeadingKeep;
itr->second->mHisDataReal.push_back(hisData);
itr->second->mHisData.push_back(hisData);
......@@ -103,6 +147,9 @@ std::shared_ptr<SensorDevice> FusionData::genSensDevice(TOBJPtr& tObjPtr)
sensDevPtr->mID = devID;
sensDevPtr->mLLastDataMS = tObjPtr->mMS;
sensDevPtr->mLastDataMS = tObjPtr->mMS;
sensDevPtr->mMaxMs = tObjPtr->mMS;
sensDevPtr->mArriveLastMs = tObjPtr->mArriveMS;
sensDevPtr->mArriveLLastMs = tObjPtr->mArriveMS;
sensDevPtr->mFrameIdx = 1;
mSensDeviceMap[devID] = sensDevPtr;
itr = mSensDeviceMap.find(devID);
......@@ -112,11 +159,22 @@ std::shared_ptr<SensorDevice> FusionData::genSensDevice(TOBJPtr& tObjPtr)
if (itr == mSensDeviceMap.end()) {
return nullptr;
}
//使用这个的前提是：每个传感器的每一帧里面的目标时间戳都一样
if (itr->second->mLastDataMS < tObjPtr->mMS) {
itr->second->mFrameIdx++;
itr->second->mLLastDataMS = itr->second->mLastDataMS;
}
//新加代码
if(itr->second->mMaxMs < tObjPtr->mMS) {
itr->second->mMaxMs = tObjPtr->mMS;
}
//更新上一帧数据
if(itr->second->mArriveLastMs < tObjPtr->mArriveMS){
//itr->second->mFrameIdx++;
itr->second->mArriveLLastMs = itr->second->mArriveLastMs;
}
itr->second->mArriveLastMs = tObjPtr->mArriveMS;
//新加结束
itr->second->mLastDataMS = tObjPtr->mMS;
itr->second->mLastDataLocalMS = mills_UTC();
itr->second->mDataDelayMS = itr->second->mLastDataLocalMS - tObjPtr->mMS;
......@@ -227,6 +285,8 @@ ENTrafficObjectType FusionData::typeStr2ID(string typeStr)
enType = enTOTypeTruck;
} else if (typeStr == "Bus") {
enType = enTOTypeBus;
} else if(typeStr == "Tricycle") {
enType = enTOTypeTricycle;
}
return enType;
......
src/fusionDistribTask.cpp
View file @ 8db78272
This diff is collapsed.
src/fusionObject.cpp
View file @ 8db78272
......@@ -67,7 +67,7 @@ void FusionObject::setInValid()
}
}
void FusionObject::setHisPoint(jfx_common_msgs::OutFusionObj& obj)
void FusionObject::setHisPoint(jfx_common_msgs::OutFusionObj& obj, ENTrafficSensMode mode)
{
STObjTrajPoint p;
p.heading = obj.heading;
......@@ -75,28 +75,34 @@ void FusionObject::setHisPoint(jfx_common_msgs::OutFusionObj& obj)
p.lon = obj.lon;
p.lat = obj.lat;
p.ms = obj.time;
p.mode = mode;
mFusionHisData.push_back(p);
if (mFusionHisData.size() > 10) {
mFusionHisData.erase(mFusionHisData.begin());
}
}
void FusionObject::InitDefaultTypeStr()
{
//查看毫米波雷达是否有Bus类型，有就赋值给默认字段mDefaultType
std::map<uint64_t, TOBJPtr>::iterator itr = mRTObjMap.begin();
while (itr != mRTObjMap.end()) {
if(itr->second->mTypeStr == "Bus"){
mDefaultType = "Bus";
break;
}
itr++;
}
}
std::string FusionObject::getTrueTypeStr()
{
//先使用哪个默认值维持这个类型：
if(mDefaultLidarType !="")
if(!mDefaultType.empty())
{
return mDefaultLidarType;
}
if(mDefaultRadarType != "")
{
return mDefaultRadarType;
}
if(mDefaultVideoType != "")
{
return mDefaultVideoType;
return mDefaultType;
}
//优先视觉，然后激光雷达，最后毫米波 雷达
std::map<uint64_t, TOBJPtr>::iterator itr = mVTObjMap.begin();
if (itr != mVTObjMap.end()) {
......@@ -124,19 +130,19 @@ double FusionObject::getHeading(double curLon, double curLat, double curSpeed)
double dist = jfx::GetDistance(mLastCalcuHeadingLon, mLastCalcuHeadingLat, curLon, curLat);
//if (dist < 3) {
if (dist < 3 || curSpeed < 2.025) {
//if (dist < 3 || curSpeed < 2.15) {
if (dist < 2 || curSpeed < 1.4) {
return mHeading;
}
//int pos = mFusionHisData.size() > 5 ? mFusionHisData.size() - 5 : 0;
//STObjTrajPoint& hisPoint = mFusionHisData[pos];
STObjTrajPoint& hisPoint = mFusionHisData[0];
//mHeading = jfx::GetDirection1(hisPoint.lon, hisPoint.lat, curLon, curLat);-->nan
//int pos = mFusionHisData.size() > 2 ? mFusionHisData.size() - 2 : 0;
int pos = mFusionHisData.size() > 5 ? mFusionHisData.size() - 5 : 0;
STObjTrajPoint& hisPoint = mFusionHisData[pos];
mHeading = FusionAlgo::geoGetHeading(hisPoint.lon, hisPoint.lat, curLon, curLat);
mLastCalcuHeadingLon = curLon;
mLastCalcuHeadingLat = curLat;
return mHeading;
}
\ No newline at end of file
}
src/fusionTrajectionTask.cpp
View file @ 8db78272
This diff is collapsed.
src/trafficObject.cpp
View file @ 8db78272
......@@ -23,7 +23,8 @@ double TrafficObject::genHeading(double speedX, int steps/* = 10*/)
double dist = jfx::GetDistance(mLon, mLat, mLastCalcuHeadingLon, mLastCalcuHeadingLat);
//if (dist < 2) {
if(dist < 3.0 || speedX < 2.025){
//if(dist < 3.0 || speedX < 2.15){
if(dist < 2.0 || speedX < 1.4){
return mHeading;
}
......@@ -41,13 +42,27 @@ double TrafficObject::genHeading(double speedX, int steps/* = 10*/)
}
}
}
//area航向保持
if (isVehicle()) {
auto itr = FConfPtr->m_heading_area.begin();
for (; itr != FConfPtr->m_heading_area.end(); ++itr) {
std::vector<double> pt;
pt.push_back(mLon);
pt.push_back(mLat);
int ret = FusionAlgo::checkInArea(itr->second, pt);
if(ret == 1)//1在多边形里面
{
mIsUseHeadingKeep = true;
return itr->first;
}
}
}
mIsUseHeadingKeep = false;
int hisCount = (int)mHisDataReal.size();
int pos = min(hisCount, steps);
STObjTrajPoint hisData1 = mHisDataReal[hisCount - pos];
//STObjTrajPoint hisData1 = mHisDataReal[0];
return FusionAlgo::geoGetHeading(hisData1.lon, hisData1.lat, mLon, mLat);
}
......@@ -126,7 +141,7 @@ double TrafficObject::genSpeedX()
bool TrafficObject::isVehicle()
{
if (mType == enTOTypePerson || mType == enTOTypeBicycle || mType == enTOTypeMotorbike) {
if (mType == enTOTypePerson || mType == enTOTypeBicycle || mType == enTOTypeMotorbike || mType == enTOTypeTricycle) {
return false;
}
return true;
......@@ -194,6 +209,16 @@ bool TrafficObject::isFarObj(std::shared_ptr<TrafficObject>& tObjPtr)
return true;
}
bool TrafficObject::isNearObj(std::shared_ptr<TrafficObject>& tObjPtr)
{
auto nearObjItr = mNearObjMap.find(tObjPtr->mKey);
if (nearObjItr == mNearObjMap.end()) {
return false;
}
return true;
}
void TrafficObject::removeNearObj(uint64_t key)
{
std::lock_guard<std::mutex> guardNearObj(mNearObjMapMutex);
......@@ -243,23 +268,28 @@ STObjTrajPoint* TrafficObject::getTrajPointClosestToMS(uint64_t ms)
bool TrafficObject::isNearStillNear(STNearObj& nearInfo) {
if (mCount <= 0) {
ROS_INFO("stillnear0: %s,%llu,%s,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id);
return false;
}
// At least 70% point near( 主要是时间延迟，车前后的情况导致匹配点少 )
if (((double)nearInfo.count / mCount) < 0.7 && nearInfo.lastMissCount > 10) {
ROS_INFO("stillnear1: %s,%llu,%s,%llu,%llu,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id,mCount,nearInfo.count);
return false;
}
if (((double)nearInfo.count / mCount) < 0.5 && nearInfo.lastMissCount > 5) {
ROS_INFO("stillnear2: %s,%llu,%s,%llu,%llu,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id,mCount,nearInfo.count);
return false;
}
if (((double)nearInfo.count / mCount) < 0.2) {
ROS_INFO("stillnear3: %s,%llu,%s,%llu,%llu,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id,mCount,nearInfo.count);
return false;
}
if (nearInfo.lastMissCount >= 20) {
ROS_INFO("stillnear4: %s,%llu,%s,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id);
return false;
}
......@@ -268,6 +298,7 @@ bool TrafficObject::isNearStillNear(STNearObj& nearInfo) {
if (tObjNear) {
double dist = jfx::GetDistance(mLon, mLat, tObjNear->mLon, tObjNear->mLat);
if (dist > 15) {
ROS_INFO("stillnear5: %s,%llu,%s,%llu", mModeStr.c_str(),mID,nearInfo.devIdStr.c_str(),nearInfo.id);
return false;
}
}
......
test/test.cpp
View file @ 8db78272
......@@ -5,6 +5,26 @@
#include "fusionDistribTask.h"
#include "fusionTrajectionTask.h"
#include "GeographicLib/Geodesic.hpp"
#include "GeographicLib/GeodesicLine.hpp"
#include "GeographicLib/Constants.hpp"
#include "GeographicLib/TransverseMercator.hpp"
#include "GeographicLib/UTMUPS.hpp"
double PointsAngle(double sdLon,double sdLat,double edLon, double edLat)
{
try {
double azi1 = 0;
double azi2 = 0;
GeographicLib::Geodesic::WGS84().Inverse(sdLat, sdLon, edLat, edLon, azi1, azi2);
return azi1;
}
catch (const std::exception &e) {
std::cout << "[GeographicLib USE ERROR]" << "Caught exception: " << e.what() << "\n";
return 0;
}
}
int main(int argc, char* argv[])
{
FusionDistributeTask dist;
......@@ -14,10 +34,23 @@ int main(int argc, char* argv[])
FusionData* pData = FDataPtr;
FusionConfig* pConfig = FConfPtr;
double radius = jfx::GetDistance(118.92785834823712, 31.72848205330513, 118.9278562835458, 31.72848003207186);
printf("radius:%lf\n",radius);
double heading = PointsAngle(120.781188,31.598892,120.781169,31.59915);
printf("heading:%lf\n",heading);
/*
double direct = jfx::GetDirection2(120.7801293,31.5994471,120.7806399,31.5994442) + 1.26;
direct = direct > 360 ? (direct - 360) : direct;
printf("heading--old:%lf\n",direct);
*/
return 0;
if (pConfig->init("/workspace/etc/rvfusion_config.yaml") != 0) {
return -1;
}
dist.init();
traj.init();
FusionCsvFile csv;
......@@ -37,7 +70,7 @@ int main(int argc, char* argv[])
TOBJPtr tStoreObj = dist.doTObj(tObj);
if (tStoreObj) {
STCheckNearItem item(tStoreObj);
if (near.checkNear(item) == 0) {
if (near.checkNear(item) == 0 && item.pos >= FConfPtr->mObjFusionMinPoint) {
FOBJPtr fObj = traj.genFusionObj(item);
if (fObj) {
if (tStoreObj->mMS > traj.mLastFusionDataMS) {
......@@ -51,8 +84,7 @@ int main(int argc, char* argv[])
tObj = csv.readLine();
}
sleep(1);
pConfig->mBAppExit = true;
//sleep(1);
sleep(1);
return 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