Skip to content
Projects
Groups
Snippets
Help
Loading...
Sign in / Register
Toggle navigation
J
jfx_rvfusion_cx
Project
Project
Details
Activity
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Board
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Packages
Packages
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
ChenKun
jfx_rvfusion_cx
Commits
8db78272
Commit
8db78272
authored
Jul 31, 2023
by
alexander
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
融合数据代码更新
parent
669f7a44
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
697 additions
and
307 deletions
+697
-307
CMakeLists.txt
CMakeLists.txt
+4
-1
fusionAlgo.h
incl/fusionAlgo.h
+5
-0
fusionConfig.h
incl/fusionConfig.h
+78
-37
fusionDataLog.h
incl/fusionDataLog.h
+5
-4
fusionDef.h
incl/fusionDef.h
+3
-1
fusionDistribTask.h
incl/fusionDistribTask.h
+6
-3
fusionObject.h
incl/fusionObject.h
+8
-5
sensDevice.h
incl/sensDevice.h
+4
-0
trafficObject.h
incl/trafficObject.h
+8
-2
fusionAlgo.cpp
src/fusionAlgo.cpp
+81
-55
fusionCheckNearTask.cpp
src/fusionCheckNearTask.cpp
+29
-5
fusionData.cpp
src/fusionData.cpp
+69
-9
fusionDistribTask.cpp
src/fusionDistribTask.cpp
+161
-56
fusionObject.cpp
src/fusionObject.cpp
+25
-20
fusionTrajectionTask.cpp
src/fusionTrajectionTask.cpp
+142
-103
trafficObject.cpp
src/trafficObject.cpp
+34
-3
test.cpp
test/test.cpp
+35
-3
No files found.
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
->
m
Height
,
tObjPtr
->
m
Length
,
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
if
DropFrameTim
e
(
std
::
shared_ptr
<
TrafficObject
>&
tObjPtr
);
bool
if
CorrectTyp
e
(
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
mDefault
Lidar
Type
=
""
;
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
...
...
@@ -25,6 +25,21 @@ FusionDistributeTask::~FusionDistributeTask()
{
}
void
FusionDistributeTask
::
init
()
{
std
::
string
dir
=
FConfPtr
->
m_root_dir
+
FConfPtr
->
m_pro_dir
;
std
::
cout
<<
"load map dir:"
<<
dir
<<
",map_cfg:"
<<
FConfPtr
->
m_map_cfg
<<
endl
;
if
(
FConfPtr
->
m_check_in_map
==
true
)
{
std
::
cout
<<
"being Load Map......"
<<
std
::
endl
;
m_OfflineMap
.
Init
(
dir
,
FConfPtr
->
m_map_cfg
);
}
else
{
std
::
cout
<<
"Not Need Load Map !!!"
<<
std
::
endl
;
}
}
int
FusionDistributeTask
::
init
(
ros
::
NodeHandle
&
nh
,
string
replayCsv
)
{
if
(
replayCsv
.
size
()
>
0
&&
mCsvFile
.
openRead
(
replayCsv
)
==
0
)
{
...
...
@@ -35,7 +50,15 @@ int FusionDistributeTask::init(ros::NodeHandle& nh, string replayCsv)
mOriginDataLog
.
init
(
FConfPtr
->
mLogPath
,
"fusion_tobj"
);
std
::
string
dir
=
FConfPtr
->
m_root_dir
+
FConfPtr
->
m_pro_dir
;
std
::
cout
<<
"load map dir:"
<<
dir
<<
",map_cfg:"
<<
FConfPtr
->
m_map_cfg
<<
endl
;
m_OfflineMap
.
Init
(
dir
,
FConfPtr
->
m_map_cfg
);
if
(
FConfPtr
->
m_check_in_map
==
true
)
{
std
::
cout
<<
"being Load Map......"
<<
std
::
endl
;
m_OfflineMap
.
Init
(
dir
,
FConfPtr
->
m_map_cfg
);
}
else
{
std
::
cout
<<
"Not Need Load Map !!!"
<<
std
::
endl
;
}
int
clusterTaskNum
=
min
(
16
,
max
(
1
,
FConfPtr
->
mClusterThreadNum
));
//一个目标分配都一个线程中
for
(
int
i
=
0
;
i
<
clusterTaskNum
;
i
++
)
{
...
...
@@ -101,13 +124,13 @@ void FusionDistributeTask::onRadarData(const jfx_common_msgs::RadarData& data)
return
;
}
uint64_t
arrive_time
=
mills_UTC
();
for
(
const
auto
&
obj
:
data
.
objs
)
{
std
::
shared_ptr
<
TrafficObject
>
tObjPtr
=
std
::
make_shared
<
TrafficObject
>
(
enTSModeRadar
);
if
(
!
tObjPtr
)
{
return
;
}
tObjPtr
->
mArriveMS
=
arrive_time
;
tObjPtr
->
mDevIDStr
=
data
.
device
;
tObjPtr
->
mModeStr
=
"r"
;
tObjPtr
->
mIDStr
=
obj
.
id
;
...
...
@@ -174,13 +197,13 @@ void FusionDistributeTask::onLidarData(const jfx_common_msgs::det_tracking_array
if
(
data
.
array
.
size
()
<=
0
)
{
return
;
}
uint64_t
arrive_time
=
mills_UTC
();
for
(
const
auto
&
obj
:
data
.
array
)
{
std
::
shared_ptr
<
TrafficObject
>
tObjPtr
=
std
::
make_shared
<
TrafficObject
>
(
enTSModeLidar
);
if
(
!
tObjPtr
)
{
return
;
}
tObjPtr
->
mArriveMS
=
arrive_time
;
tObjPtr
->
mDevIDStr
=
obj
.
device
;
tObjPtr
->
mID
=
obj
.
obj_id
;
tObjPtr
->
mModeStr
=
"l"
;
...
...
@@ -203,9 +226,14 @@ void FusionDistributeTask::onLidarData(const jfx_common_msgs::det_tracking_array
tObjPtr
->
mTypeStr
=
"Car"
;
}
else
if
(
"truck"
==
obj
.
name
)
{
tObjPtr
->
mTypeStr
=
"Truck"
;
}
else
if
(
"cyclist"
==
obj
.
name
)
{
if
(
tObjPtr
->
mLength
>=
10.0
){
tObjPtr
->
mTypeStr
=
"Bus"
;
}
}
else
if
(
"cyclist"
==
obj
.
name
)
{
//两轮车:包括自行车和摩托车,电动车
tObjPtr
->
mTypeStr
=
"Motorbike"
;
}
else
{
}
else
if
(
"other"
==
obj
.
name
)
{
//三轮车
tObjPtr
->
mTypeStr
=
"Tricycle"
;
}
else
{
tObjPtr
->
mTypeStr
=
"UnKnown"
;
}
tObjPtr
->
mSubType
=
0
;
...
...
@@ -265,6 +293,7 @@ void FusionDistributeTask::udpSrv()
tObjPtr
->
mHeight
=
obj
.
vehH
;
tObjPtr
->
mXPos
=
obj
.
xPos
;
tObjPtr
->
mYPos
=
obj
.
yPos
;
tObjPtr
->
mTypeStr
=
obj
.
type
;
tObjPtr
->
mSubType
=
0
;
tObjPtr
->
mProb
=
obj
.
prop
;
...
...
@@ -356,14 +385,13 @@ std::shared_ptr<TrafficObject> FusionDistributeTask::doTObj(std::shared_ptr<Traf
return
nullptr
;
}
/*不抽帧*/
/*
//抽帧
if
(
ifDropFrame
(
sensDevicePtr
,
nextQueueSize
))
{
return
nullptr
;
}
*/
//
降低数据密度,也是抽帧
if
(
if
DropFrameTim
e
(
tObjPtr
)){
//
更新类型
if
(
if
CorrectTyp
e
(
tObjPtr
)){
return
nullptr
;
}
...
...
@@ -376,7 +404,7 @@ std::shared_ptr<TrafficObject> FusionDistributeTask::doTObj(std::shared_ptr<Traf
if
(
filterInMap
(
tObjPtr
)){
return
nullptr
;
}
std
::
cout
<<
"OK MAP ID:"
<<
tObjPtr
->
mID
<<
","
<<
tObjPtr
->
mModeStr
<<
endl
;
//
std::cout<<"OK MAP ID:"<<tObjPtr->mID<<","<<tObjPtr->mModeStr<<endl;
// set obj param
tObjPtr
->
mDevID
=
sensDevicePtr
->
mID
;
tObjPtr
->
mMode
=
sensDevicePtr
->
mMode
;
...
...
@@ -387,12 +415,17 @@ std::shared_ptr<TrafficObject> FusionDistributeTask::doTObj(std::shared_ptr<Traf
FConfPtr
->
mCrossCenterX
,
FConfPtr
->
mCrossCenterY
);
}
//过滤多边形
if
(
filterArea
(
tObjPtr
)){
return
nullptr
;
}
if
(
filter
(
tObjPtr
))
{
FDataPtr
->
mFusionState
.
filteredCount
++
;
return
nullptr
;
}
std
::
cout
<<
"START FUSION ID:"
<<
tObjPtr
->
mID
<<
","
<<
tObjPtr
->
mModeStr
<<
endl
;
//
std::cout<<"START FUSION ID:"<<tObjPtr->mID<<","<<tObjPtr->mModeStr<<endl;
// ToDO: check lost 放到cluster里
// ToDO: 增加目标抽正设置
//tObjPtr是一个点一个对象,
...
...
@@ -440,39 +473,44 @@ bool FusionDistributeTask::ifDropFrame(SDEVPtr& sensDevPtr, int checkNearQueueLe
return
false
;
}
bool
FusionDistributeTask
::
if
DropFrameTim
e
(
std
::
shared_ptr
<
TrafficObject
>&
tObjPtr
)
bool
FusionDistributeTask
::
if
CorrectTyp
e
(
std
::
shared_ptr
<
TrafficObject
>&
tObjPtr
)
{
if
(
!
tObjPtr
)
{
if
(
!
tObjPtr
)
{
return
true
;
}
if
(
FConfPtr
->
m
IfDynamicChangeDropFrameStep
==
false
)
if
(
FConfPtr
->
m
_correct_type
==
false
)
{
return
false
;
}
/*
if(mCheckTObject.size() > 1000)
{
mCheckTObject.clear();
static
uint64_t
lastClearMs
=
mills_UTC
();
uint64_t
curMs
=
mills_UTC
();
//每隔5分钟清理一次
if
((
curMs
-
lastClearMs
)
>
300000
)
{
lastClearMs
=
curMs
;
auto
itr
=
mCheckTObject
.
begin
();
for
(;
itr
!=
mCheckTObject
.
end
();)
{
if
((
curMs
-
itr
->
second
.
mLastTime
)
>=
300000
)
{
mCheckTObject
.
erase
(
itr
++
);
continue
;
}
itr
++
;
}
}
*/
//以第一次出现的为准
std
::
string
key
=
tObjPtr
->
mDevIDStr
+
"-"
+
std
::
to_string
(
tObjPtr
->
mID
);
auto
iter
=
mCheckTObject
.
find
(
key
);
if
(
iter
!=
mCheckTObject
.
end
()){
if
(
fabs
(
tObjPtr
->
mMS
-
iter
->
second
.
mLastTime
)
<
60
){
std
::
cout
<<
"filte:"
<<
key
<<
"-"
<<
tObjPtr
->
mTypeStr
<<
std
::
endl
;
return
true
;
}
else
{
iter
->
second
.
mLastTime
=
tObjPtr
->
mMS
;
}
if
(
iter
->
second
.
mTypeStr
!=
tObjPtr
->
mTypeStr
)
{
tObjPtr
->
mTypeStr
=
iter
->
second
.
mTypeStr
;
}
}
else
{
STCheckTObject
checkTobject
;
checkTobject
.
mLastTime
=
tObjPtr
->
mMS
;
checkTobject
.
mLastTime
=
curMs
;
checkTobject
.
mTypeStr
=
tObjPtr
->
mTypeStr
;
mCheckTObject
[
key
]
=
checkTobject
;
}
...
...
@@ -486,19 +524,56 @@ bool FusionDistributeTask::filterRoi(std::shared_ptr<TrafficObject>& tObjPtr)
return
true
;
}
STLine
curLine
;
curLine
.
x1
=
tObjPtr
->
mLon
;
curLine
.
y1
=
tObjPtr
->
mLat
;
curLine
.
x2
=
FConfPtr
->
mCrossCenterX
;
curLine
.
y2
=
FConfPtr
->
mCrossCenterY
;
if
(
FConfPtr
->
m_check_roi
==
false
)
{
return
false
;
}
if
(
FConfPtr
->
m_roi_area
.
size
()
>
1
)
{
std
::
vector
<
double
>
pt
;
pt
.
push_back
(
tObjPtr
->
mLon
);
pt
.
push_back
(
tObjPtr
->
mLat
);
int
ret
=
FusionAlgo
::
checkInArea
(
FConfPtr
->
m_roi_area
,
pt
);
if
(
ret
==
0
)
{
return
true
;
}
}
return
false
;
}
bool
FusionDistributeTask
::
filterArea
(
std
::
shared_ptr
<
TrafficObject
>&
tObjPtr
)
{
if
(
!
tObjPtr
)
{
return
true
;
}
for
(
size_t
i
=
0
;
i
<
FConfPtr
->
mFilterLine
.
size
();
i
++
)
if
(
FConfPtr
->
m_lidar_inner_area
.
size
()
>
1
&&
tObjPtr
->
mMode
==
enTSModeLidar
)
{
if
(
FusionAlgo
::
intersection
(
curLine
,
FConfPtr
->
mFilterLine
[
i
]))
std
::
vector
<
double
>
pt
;
pt
.
push_back
(
tObjPtr
->
mLon
);
pt
.
push_back
(
tObjPtr
->
mLat
);
int
ret
=
FusionAlgo
::
checkInArea
(
FConfPtr
->
m_lidar_inner_area
,
pt
);
if
(
ret
==
0
)
{
return
true
;
}
}
if
(
FConfPtr
->
m_radar_outer_area
.
size
()
>
1
&&
tObjPtr
->
mMode
==
enTSModeRadar
)
{
std
::
vector
<
double
>
pt
;
pt
.
push_back
(
tObjPtr
->
mLon
);
pt
.
push_back
(
tObjPtr
->
mLat
);
int
ret
=
FusionAlgo
::
checkInArea
(
FConfPtr
->
m_radar_outer_area
,
pt
);
if
(
ret
==
1
)
{
return
true
;
}
}
return
false
;
}
...
...
@@ -508,15 +583,19 @@ bool FusionDistributeTask::filterInMap(std::shared_ptr<TrafficObject>& tObjPtr)
{
return
true
;
}
//地图检查并且是激光雷达的情况下
if
(
FConfPtr
->
m_check_in_map
==
true
&&
tObjPtr
->
mMode
==
enTSModeLidar
)
if
(
FConfPtr
->
m_check_in_map
==
true
)
{
double
_l
on
=
0
,
_lat
=
0
,
_l
aneAngle
=
0
;
int
isInmap
=
IsInMap
(
tObjPtr
->
mXPos
,
tObjPtr
->
mYPos
,
tObjPtr
->
mZPos
,
tObjPtr
->
mHeading
,
_lon
,
_l
at
,
_laneAngle
);
double
_laneAngle
=
0
;
int
isInmap
=
IsInMap
V2
(
tObjPtr
->
mHeading
,
tObjPtr
->
mLon
,
tObjPtr
->
mL
at
,
_laneAngle
);
if
(
isInmap
!=
0
)
{
return
true
;
}
else
{
tObjPtr
->
mLaneAngle
=
_laneAngle
;
}
}
return
false
;
}
...
...
@@ -540,7 +619,6 @@ int FusionDistributeTask::IsInMap(float x, float y, float z, float rot_y, double
std
::
vector
<
long
>
vctlOutPreRoadId
;
std
::
vector
<
long
>
vctlOutNxtRoadId
;
//ROS_INFO("begin call jfxmap");
bool
result
=
m_OfflineMap
.
GetMapData
(
ptInLoc
,
dCarAngle
,
lOutRaodId
,
vctlOutPreRoadId
,
vctlOutNxtRoadId
,
nLaneCnt
,
nOutLaneNum
,
nOutLaneType
,
nOutLeftEdgeCrossType
,
nOutRightEdgeCrossType
,
...
...
@@ -556,6 +634,40 @@ int FusionDistributeTask::IsInMap(float x, float y, float z, float rot_y, double
}
}
int
FusionDistributeTask
::
IsInMapV2
(
float
rot_y
,
double
lon
,
double
lat
,
double
&
laneAngle
)
{
//计算道路朝向
int
nLaneCnt
=
0
/*所在道路的车道总数*/
,
nOutLaneNum
=
0
;
//当前所在位置的车道序号,从左至右,从1开始
jf
::
LaneType
nOutLaneType
=
jf
::
LaneType
::
NotInvestigated
;
//车道类型,看头文件枚举
jf
::
EdgeCrossType
nOutLeftEdgeCrossType
;
//左车道边缘线的跨越属性,具体看头文件枚举
jf
::
EdgeCrossType
nOutRightEdgeCrossType
;
//右车道边缘线的跨越属性,具体看头文件枚举
int
nOutSpeedLimit
=
0
;
//车道最高限速值,单位是千米每小时
double
dOutLaneAngle
=
0.0
;
//所在车道,车应该的朝向角度,北为0,顺时针旋转360,0-360
const
jf
::
Point
ptInLoc
=
{
lon
,
lat
,
0.0
};
jf
::
Point
ptOutFoot
;
//车道中心线做垂线的点
double
dCarAngle
=
rot_y
/
M_PI
*
180
;
//角度值
dCarAngle
=
FConfPtr
->
m_lidar_x_angle
-
dCarAngle
;
long
lOutRaodId
;
std
::
vector
<
long
>
vctlOutPreRoadId
;
std
::
vector
<
long
>
vctlOutNxtRoadId
;
bool
result
=
m_OfflineMap
.
GetMapData
(
ptInLoc
,
dCarAngle
,
lOutRaodId
,
vctlOutPreRoadId
,
vctlOutNxtRoadId
,
nLaneCnt
,
nOutLaneNum
,
nOutLaneType
,
nOutLeftEdgeCrossType
,
nOutRightEdgeCrossType
,
nOutSpeedLimit
,
dOutLaneAngle
,
ptOutFoot
);
if
(
result
)
{
laneAngle
=
dOutLaneAngle
;
return
0
;
}
else
{
return
-
1
;
}
}
bool
FusionDistributeTask
::
filter
(
std
::
shared_ptr
<
TrafficObject
>&
tObjPtr
)
{
if
(
!
tObjPtr
)
{
...
...
@@ -586,32 +698,30 @@ bool FusionDistributeTask::filter(std::shared_ptr<TrafficObject>& tObjPtr)
return
true
;
}
//
融合范围设定一个值
//
超过最大融合范围,过滤掉
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mFilterMaxRadius
)
{
return
true
;
}
//
机动车大于50m,不要激光雷达
//
激光雷达机动&非机动
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mLidarVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeLidar
&&
tObjPtr
->
isVehicle
())
{
return
true
;
}
//非机动大于一个融合范围130米
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mLidarUnVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeLidar
&&
!
tObjPtr
->
isVehicle
())
{
return
true
;
}
//机动车小于30m不要毫米波雷达
if
(
tObjPtr
->
mDist2Center
<
FConfPtr
->
mRadarVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeRadar
&&
tObjPtr
->
isVehicle
())
{
//毫米波机动&非机动
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mRadarVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeRadar
&&
tObjPtr
->
isVehicle
())
{
return
true
;
}
//非机动毫米波:大于一个最大值
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mRadarUnVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeRadar
&&
!
tObjPtr
->
isVehicle
())
{
return
true
;
}
//视觉机动&非机动
if
(
tObjPtr
->
mDist2Center
>
FConfPtr
->
mVisionVehicleOutRadius
&&
tObjPtr
->
mMode
==
enTSModeVideo
&&
tObjPtr
->
isVehicle
())
{
return
true
;
}
...
...
@@ -620,22 +730,17 @@ bool FusionDistributeTask::filter(std::shared_ptr<TrafficObject>& tObjPtr)
return
true
;
}
//设备过滤
auto
itr
=
FConfPtr
->
mFilterDevID
.
find
(
tObjPtr
->
mDevIDStr
);
if
(
itr
!=
FConfPtr
->
mFilterDevID
.
end
()
&&
itr
->
second
==
tObjPtr
->
mMode
)
{
return
true
;
}
//未知类型过滤
if
(
tObjPtr
->
mType
==
enTOTypeUnknown
)
{
return
true
;
}
if
(
tObjPtr
->
mMode
==
enTSModeRadar
&&
tObjPtr
->
mLength
>=
7
)
{
if
(
tObjPtr
->
mType
==
enTOTypeCar
)
{
tObjPtr
->
mType
=
enTOTypeTruck
;
tObjPtr
->
mTypeStr
=
"Truck"
;
}
}
return
false
;
}
...
...
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
...
...
@@ -83,6 +83,7 @@ void FusionTrajectionTask::run()
}
}
/// 找item.tObjPtr对应的FObj
FOBJPtr
FusionTrajectionTask
::
genFusionObj
(
STCheckNearItem
&
item
)
{
TOBJPtr
tObjPtr
=
item
.
tObjPtr
;
...
...
@@ -100,6 +101,7 @@ FOBJPtr FusionTrajectionTask::genFusionObj(STCheckNearItem& item)
if
(
tObjNear
&&
tObjNear
->
mFID
==
tObjPtr
->
mFID
)
{
tObjPtr
->
mFID
=
0
;
}
ROS_INFO
(
"score>0gen1: %s,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
);
return
nullptr
;
}
...
...
@@ -108,13 +110,25 @@ FOBJPtr FusionTrajectionTask::genFusionObj(STCheckNearItem& item)
tObjPtr
->
removeNearObj
(
itrNear
->
second
.
key
);
tObjPtr
->
resetMaxScoreObj
();
tObjPtr
->
mFID
=
0
;
ROS_INFO
(
"score>0gen2: %s,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
);
return
nullptr
;
}
//有这么一种情况:先A跟B连着,A没有生成B就无法生成,B跟C连着,B没有生成,C就无法生成,C跟D连着,C没有生成D就无法生成
//导致ABCD无法连城一条完整的线
//修改逻辑,没有生成的情况下,重置最大分数
if
(
tObjNear
->
mFID
==
0
)
{
ROS_INFO
(
"score>0gen31: %s,%llu,%llu,%llu,%d,%d,%d"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
,
tObjNear
->
mID
,
tObjNear
->
mCheckNearPos
,
tObjNear
->
mCount
,
tObjNear
->
mBLost
);
if
(
tObjNear
->
mCheckNearPos
<
FConfPtr
->
mObjFusionMinPoint
){
tObjPtr
->
resetMaxScoreObj
();
}
else
{
ROS_INFO
(
"score>0gen32: %s,%llu,%llu,%llu,%d,%d,%d"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
,
tObjNear
->
mID
,
tObjNear
->
mCheckNearPos
,
tObjNear
->
mCount
,
tObjNear
->
mBLost
);
}
return
nullptr
;
}
/*
//isNearStillNear在这种很小空间的不适用
if (tObjPtr->isNearStillNear(itrNear->second) == false) {
if (tObjNear && tObjNear->mBLost == false && (tObjPtr->mFusionMS < (tObjNear->mFusionMS + 500))) {
tObjPtr->removeNearObj(itrNear->second.key);
...
...
@@ -122,13 +136,16 @@ FOBJPtr FusionTrajectionTask::genFusionObj(STCheckNearItem& item)
if (tObjNear && tObjNear->mFID == tObjPtr->mFID) {
tObjPtr->mFID = 0;
}
ROS_INFO("score>0gen4: %s,%llu,%llu,%llu,%llu,%llu", tObjPtr->mModeStr.c_str(),checkPoint.ms,tObjPtr->mID,
tObjPtr->mFusionMS,tObjNear->mID,tObjNear->mFusionMS);
return nullptr;
}
}
*/
auto
fObjPtr
=
FDataPtr
->
getFusionObj
(
tObjNear
->
mFID
);
if
(
!
fObjPtr
)
{
tObjNear
->
mFID
=
0
;
ROS_INFO
(
"score>0gen5: %s,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
);
return
nullptr
;
}
...
...
@@ -137,25 +154,38 @@ FOBJPtr FusionTrajectionTask::genFusionObj(STCheckNearItem& item)
if
(
fObjPtrOld
)
{
fObjPtrOld
->
removeTObj
(
tObjPtr
);
}
ROS_INFO
(
"score>0gen61: %s,%llu,%llu,%llu,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
,
tObjPtr
->
mFID
,
tObjNear
->
mID
,
tObjNear
->
mFID
);
fObjPtr
->
setTObj
(
tObjPtr
);
ROS_INFO
(
"score>0gen62: %s,%llu,%llu,%llu,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
,
tObjPtr
->
mFID
,
tObjNear
->
mID
,
tObjNear
->
mFID
);
}
}
if
(
tObjPtr
->
mFID
==
0
)
{
if
(
tObjPtr
->
mCount
<
FConfPtr
->
mObjFusionMinPoint
)
{
ROS_INFO
(
"score=0gen1: %d,%s,%llu,%llu"
,
tObjPtr
->
mCount
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
);
return
nullptr
;
}
// 活动范围小的不创建FusionObj
double
activeRadius
=
jfx
::
GetDistance
(
tObjPtr
->
mHisData
[
0
].
lon
,
tObjPtr
->
mHisData
[
0
].
lat
,
checkPoint
.
lon
,
checkPoint
.
lat
);
/* 输出的时候再判断
if ((tObjPtr->isVehicle() && activeRadius < 3) || (!tObjPtr->isVehicle() && activeRadius < 0.5)) {
return nullptr;
/*
if(tObjPtr->mMode == enTSModeLidar){
if ((tObjPtr->isVehicle() && activeRadius < 2) || (!tObjPtr->isVehicle() && activeRadius < 0.5)) {
ROS_INFO("score=0gen2: %s,%llu,%llu", tObjPtr->mModeStr.c_str(),checkPoint.ms,tObjPtr->mID);
return nullptr;
}
}
if(tObjPtr->mMode == enTSModeRadar){//毫米波没有非机动车
if ((tObjPtr->isVehicle() && activeRadius < 1.3) || (!tObjPtr->isVehicle() && activeRadius < 0.5)) {
ROS_INFO("score=0gen2: %s,%llu,%llu", tObjPtr->mModeStr.c_str(),checkPoint.ms,tObjPtr->mID);
return nullptr;
}
}
*/
//这个判断的意义何在?机动车的时候创建目标来源
if
(
FConfPtr
->
mCreateFusionObjMode
.
find
(
tObjPtr
->
mModeStr
)
==
std
::
string
::
npos
&&
tObjPtr
->
isVehicle
())
{
ROS_INFO
(
"score=0gen3: %s,%llu,%llu"
,
tObjPtr
->
mModeStr
.
c_str
(),
checkPoint
.
ms
,
tObjPtr
->
mID
);
return
nullptr
;
}
...
...
@@ -174,11 +204,8 @@ FOBJPtr FusionTrajectionTask::genFusionObj(STCheckNearItem& item)
void
FusionTrajectionTask
::
outFusionObj
(
int
pos
,
FOBJPtr
&
fObjPtr
,
TOBJPtr
&
tObjPtr
)
{
if
(
tObjPtr
->
mMS
>=
1681797808013
)
{
if
(
tObjPtr
->
mID
==
155
)
{
int
a
=
0
;
}
if
(
tObjPtr
->
mID
==
268357
)
{
if
(
tObjPtr
->
mMS
>=
1686817592742
)
{
if
(
tObjPtr
->
mID
==
2028942
)
{
int
a
=
0
;
}
}
...
...
@@ -189,73 +216,6 @@ void FusionTrajectionTask::outFusionObj(int pos, FOBJPtr& fObjPtr, TOBJPtr& tObj
void
FusionTrajectionTask
::
outFusionObj3
(
int
pos
,
FOBJPtr
&
fObjPtr
,
TOBJPtr
&
tObjPtr
)
{
fObjPtr
->
mMainObj
=
nullptr
;
if
(
FConfPtr
->
mFirstOutMode
==
enTSModeRadar
&&
FConfPtr
->
mCreateFusionObjMode
==
"rv"
)
{
if
(
FConfPtr
->
mFirstOutMode
==
enTSModeRadar
)
{
if
(
fObjPtr
->
mMainObj
==
nullptr
)
{
uint64_t
ms
=
std
::
numeric_limits
<
uint64_t
>::
max
();
for
(
auto
const
&
n
:
fObjPtr
->
mRTObjMap
)
{
if
(
n
.
second
==
nullptr
||
n
.
second
->
mOutPos
>=
n
.
second
->
mHisData
.
size
())
{
continue
;
}
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
}
}
if
(
fObjPtr
->
mMainObj
==
nullptr
)
{
uint64_t
ms
=
std
::
numeric_limits
<
uint64_t
>::
max
();
for
(
auto
const
&
n
:
fObjPtr
->
mVTObjMap
)
{
if
(
n
.
second
==
nullptr
||
n
.
second
->
mOutPos
>=
n
.
second
->
mHisData
.
size
())
{
continue
;
}
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
}
}
}
else
{
if
(
fObjPtr
->
mMainObj
==
nullptr
)
{
uint64_t
ms
=
std
::
numeric_limits
<
uint64_t
>::
max
();
for
(
auto
const
&
n
:
fObjPtr
->
mVTObjMap
)
{
if
(
n
.
second
==
nullptr
||
n
.
second
->
mOutPos
>=
n
.
second
->
mHisData
.
size
())
{
continue
;
}
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
}
}
if
(
fObjPtr
->
mMainObj
==
nullptr
)
{
uint64_t
ms
=
std
::
numeric_limits
<
uint64_t
>::
max
();
for
(
auto
const
&
n
:
fObjPtr
->
mRTObjMap
)
{
if
(
n
.
second
==
nullptr
||
n
.
second
->
mOutPos
>=
n
.
second
->
mHisData
.
size
())
{
continue
;
}
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
}
}
}
}
if
(
FConfPtr
->
mFirstOutMode
==
enTSModeLidar
&&
FConfPtr
->
mCreateFusionObjMode
==
"lr"
)
{
if
(
FConfPtr
->
mFirstOutMode
==
enTSModeLidar
)
{
...
...
@@ -268,7 +228,7 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
FConfPtr
->
m_lidar_delay
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
...
...
@@ -283,7 +243,7 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
FConfPtr
->
m_radar_delay
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
...
...
@@ -299,7 +259,7 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
FConfPtr
->
m_radar_delay
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
...
...
@@ -314,7 +274,7 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
if
(
n
.
second
->
mValid
==
false
||
n
.
second
->
mBLost
)
{
continue
;
}
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
1000
)
>
tObjPtr
->
mMS
)
{
if
(
n
.
second
->
mFMS
<
ms
&&
(
n
.
second
->
mMS
+
FConfPtr
->
m_lidar_delay
)
>
tObjPtr
->
mMS
)
{
ms
=
n
.
second
->
mFMS
;
fObjPtr
->
mMainObj
=
n
.
second
;
}
...
...
@@ -330,7 +290,28 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
// }
//}
if
(
fObjPtr
->
mMainObj
&&
fObjPtr
->
mMainObj
==
tObjPtr
)
{
bool
isCanOut
=
false
;
if
(
fObjPtr
->
mMainObj
&&
fObjPtr
->
mMainObj
==
tObjPtr
&&
fObjPtr
->
mLastOutMS
<=
fObjPtr
->
mMainObj
->
mHisData
[
fObjPtr
->
mMainObj
->
mOutPos
].
ms
){
isCanOut
=
true
;
//距离门限控制
if
(
fObjPtr
->
mLon
>
0
&&
fObjPtr
->
mLat
>
0
&&
fObjPtr
->
mMode
!=
tObjPtr
->
mMode
)
{
auto
point
=
fObjPtr
->
mMainObj
->
mHisData
[
fObjPtr
->
mMainObj
->
mOutPos
];
double
swapRadius
=
jfx
::
GetDistance
(
fObjPtr
->
mLon
,
fObjPtr
->
mLat
,
point
.
lon
,
point
.
lat
);
if
(
swapRadius
>
15
){
isCanOut
=
false
;
}
}
//统计,防止穿模,穿模这个在继续优化,不应该放在这里
/*
fObjPtr->mCount++;
if(fObjPtr->mCount > FConfPtr->mTrajNearMinPoint + 1){
isCanOut = true;
}
*/
}
if
(
isCanOut
){
int
count
=
fObjPtr
->
mMainObj
->
mHisData
.
size
();
auto
point
=
fObjPtr
->
mMainObj
->
mHisData
[
fObjPtr
->
mMainObj
->
mOutPos
];
...
...
@@ -344,8 +325,8 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
}
obj
.
all_id
=
fObjPtr
->
mID
;
obj
.
mec_id
=
FConfPtr
->
mMecID
;
obj
.
type
=
fObjPtr
->
getTrueTypeStr
().
c_str
()
;
obj
.
time
=
mLastFusionDataMS
;
//
point.ms;
//obj.time = mLastFusionDataMS; // point.ms
;
obj
.
time
=
point
.
ms
;
obj
.
Prob
=
0
;
obj
.
now_x
=
0
;
obj
.
now_y
=
0
;
...
...
@@ -362,7 +343,8 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
obj
.
inner_pos
=
fObjPtr
->
mMainObj
->
mDevIDStr
;
obj
.
is_turn
=
false
;
obj
.
lane
=
point
.
laneseqnum
;
FusionAlgo
::
fitFObjNextPoint
(
fObjPtr
,
obj
);
if
(
point
.
isUseHeadingKeep
==
false
)
{
//fObjPtr->getHeading(point.lon, point.lat);
//增加一个速度判断航向,在停车等待的时候经纬度给的不准确
...
...
@@ -371,42 +353,99 @@ void FusionTrajectionTask::outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
obj
.
heading
=
fObjPtr
->
mHeading
;
}
}
//if (fObjPtr->mMainObj->mMode == enTSModeLidar) {
// obj.heading = fObjPtr->mMainObj->mHeading;
//}
FusionAlgo
::
fitFObjNextPoint
(
fObjPtr
,
obj
);
fObjPtr
->
setHisPoint
(
obj
);
fObjPtr
->
mLastOutMS
=
mLastFusionDataMS
;
fObjPtr
->
setHisPoint
(
obj
,
tObjPtr
->
mMode
);
//类型的处理放在这里
//fObjPtr->InitDefaultTypeStr();//用毫米波的初始化Bus
if
(
FConfPtr
->
m_fusion_correct_type
==
false
){
//init default type
if
(
fObjPtr
->
mDefaultType
.
empty
())
{
fObjPtr
->
mDefaultType
=
tObjPtr
->
mTypeStr
;
}
else
{
if
(
fObjPtr
->
mIsCheck
==
false
)
{
if
(
fObjPtr
->
mDefaultType
==
"Bus"
)
{
fObjPtr
->
mIsCheck
=
true
;
}
else
if
(
fObjPtr
->
mMainObj
->
mMode
==
enTSModeLidar
)
{
fObjPtr
->
mDefaultType
=
tObjPtr
->
mTypeStr
;
fObjPtr
->
mIsCheck
=
true
;
}
}
}
}
else
{
//使用经过平滑后的点判断
//init default type
if
(
fObjPtr
->
mDefaultType
.
empty
())
{
fObjPtr
->
mDefaultType
=
tObjPtr
->
mTypeStr
;
}
else
{
if
(
fObjPtr
->
mIsCheck
==
false
)
{
if
(
fObjPtr
->
mDefaultType
==
"Bus"
)
{
fObjPtr
->
mIsCheck
=
true
;
}
else
if
(
FConfPtr
->
m_fusion_type_area
.
size
()
>
1
&&
tObjPtr
->
mMode
==
enTSModeLidar
)
{
std
::
vector
<
double
>
pt
;
pt
.
push_back
(
point
.
lon
);
pt
.
push_back
(
point
.
lat
);
int
ret
=
FusionAlgo
::
checkInArea
(
FConfPtr
->
m_fusion_type_area
,
pt
);
if
(
ret
==
1
)
{
fObjPtr
->
mDefaultType
=
tObjPtr
->
mTypeStr
;
fObjPtr
->
mIsCheck
=
true
;
}
}
}
}
}
obj
.
type
=
fObjPtr
->
getTrueTypeStr
().
c_str
();
//对Person调整航向
FusionAlgo
::
fitFObjHeading
(
fObjPtr
,
obj
);
//fObjPtr->mLastOutMS = mLastFusionDataMS;
fObjPtr
->
mLastOutFusionMs
=
mLastFusionDataMS
;
fObjPtr
->
mLastOutMS
=
point
.
ms
;
fObjPtr
->
mLastOutMainObjMS
=
fObjPtr
->
mMainObj
->
mMS
;
fObjPtr
->
mLon
=
obj
.
lon
;
fObjPtr
->
mLat
=
obj
.
lat
;
//init default type
if
(
tObjPtr
->
mMode
==
enTSModeLidar
)
{
fObjPtr
->
mDefaultLidarType
=
tObjPtr
->
mTypeStr
;
}
//输出前将时间更改,和真值CPT对比
point
.
ms
=
point
.
ms
+
FConfPtr
->
m_fusion_delay
;
FPubTaskPtr
->
pushFusionObj
(
obj
);
if
(
FConfPtr
->
mBFusionStdout
)
{
mFusionDataLog
.
save
(
obj
,
fObjPtr
);
#ifdef _DEBUG
/*
printf("%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%.10lf,%s,%ld,%s,+\n",
mLastFusionDataMS, obj.all_id, obj.type.c_str(), obj.heading, obj.lon, obj.lat, obj.speed,
fObjPtr->mMainObj->mDevIDStr.c_str(), fObjPtr->mMainObj->mID, fObjPtr->mMainObj->mModeStr.c_str());
fflush(stdout);
*/
//ROS_INFO("select: %llu,%s,%llu,%llu,%llu,%llu", obj.all_id,fObjPtr->mMainObj->mModeStr.c_str(),fObjPtr->mLastOutFusionMs,
// fObjPtr->mLastOutMainObjMS,obj.time,fObjPtr->mMainObj->mID);
#endif // _DEBUG
}
}
else
{
if
(
FConfPtr
->
mBFusionStdout
)
{
#ifdef _DEBUG
auto
point
=
tObjPtr
->
mHisData
[
tObjPtr
->
mOutPos
];
/*
printf("%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%.10lf,%s,%ld,%s,-\n",
mLastFusionDataMS, fObjPtr->mID, tObjPtr->mTypeStr.c_str(), point.heading, point.lon, point.lat, point.speed,
tObjPtr->mDevIDStr.c_str(), tObjPtr->mID, tObjPtr->mModeStr.c_str());
fflush(stdout);
*/
// ROS_INFO("refuse: %llu,%s,%llu,%llu,%llu,%llu", fObjPtr->mID,tObjPtr->mModeStr.c_str(),fObjPtr->mLastOutFusionMs,
// fObjPtr->mLastOutMainObjMS,point.ms,tObjPtr->mID);
#endif // _DEBUG
}
}
...
...
@@ -516,9 +555,9 @@ void FusionTrajectionTask::outFusionObj1(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
obj
.
inner_pos
=
fObjPtr
->
mMainObj
->
mDevIDStr
;
obj
.
is_turn
=
false
;
obj
.
lane
=
point
.
laneseqnum
;
FusionAlgo
::
fitFObjNextPoint
(
fObjPtr
,
obj
);
fObjPtr
->
setHisPoint
(
obj
);
fObjPtr
->
setHisPoint
(
obj
,
tObjPtr
->
mMode
);
FPubTaskPtr
->
pushFusionObj
(
obj
);
...
...
@@ -650,9 +689,9 @@ void FusionTrajectionTask::outFusionObj2(int pos, FOBJPtr& fObjPtr, TOBJPtr& tOb
obj
.
cross_name
=
""
;
obj
.
inner_pos
=
fObjPtr
->
mMainObj
->
mDevIDStr
;
obj
.
is_turn
=
false
;
FusionAlgo
::
fitFObjNextPoint
(
fObjPtr
,
obj
);
fObjPtr
->
setHisPoint
(
obj
);
fObjPtr
->
setHisPoint
(
obj
,
tObjPtr
->
mMode
);
FPubTaskPtr
->
pushFusionObj
(
obj
);
...
...
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
;
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment