Skip to content
Projects
Groups
Snippets
Help
Loading...
Sign in / Register
Toggle navigation
L
localize_for_ppk
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
wangdawei
localize_for_ppk
Commits
73904574
Commit
73904574
authored
May 30, 2023
by
wangdawei
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
test
parent
d1b65e57
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
13 additions
and
13 deletions
+13
-13
adjust_ppk.cpp
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
+12
-12
adjust_ppk.h
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h
+1
-1
No files found.
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
View file @
73904574
...
...
@@ -281,9 +281,9 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
for
(;
ppkIndex_
<
localPoseVec_
.
size
();
ppkIndex_
++
){
const
auto
&
pose
=
localPoseVec_
.
at
(
ppkIndex_
);
if
(
0
==
ppkIndex_
){
mapPose_
=
localPoseVec_
.
front
().
pose
.
cast
<
float
>
()
;
mapPose_
=
localPoseVec_
.
front
().
pose
;
}
else
{
mapPose_
=
mapPose_
*
(
localPoseVec_
.
at
(
ppkIndex_
-
1
).
pose
.
inverse
()
*
pose
.
pose
)
.
cast
<
float
>
()
;
mapPose_
=
mapPose_
*
(
localPoseVec_
.
at
(
ppkIndex_
-
1
).
pose
.
inverse
()
*
pose
.
pose
);
}
if
(
pose
.
timestamp
>
backPTime
){
break
;
...
...
@@ -361,33 +361,33 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
}
static
std
::
ofstream
ofs
(
ieBaseDir_
+
"/CloudPredictPose.txt"
,
ios
::
app
);
Quaternion
f
q
(
mapPose_
.
linear
());
Vector3
f
rpy
=
RotationQuaternionToEulerVector
(
q
);
Quaternion
d
q
(
mapPose_
.
linear
());
Vector3
d
rpy
=
RotationQuaternionToEulerVector
(
q
);
LOG
(
INFO
)
<<
setprecision
(
15
)
<<
"cloud time: "
<<
cloudInfo
.
timestamp
<<
" guessPose: "
<<
mapPose_
.
translation
().
transpose
()
<<
" rpy: "
<<
rpy
.
transpose
();
// pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
GnssPoint
finalGnssPose
;
GuessPose
guessPose
;
guessPose
.
map_point
=
mapPose_
.
cast
<
double
>
()
;
guessPose
.
map_point
=
mapPose_
;
auto
increaseRpy
=
RotationQuaternionToEulerVector
(
Quaterniond
(
cloudInfo
.
filterPoseInfo
.
increasePose
.
linear
()));
guessPose
.
movement
=
cloudInfo
.
filterPoseInfo
.
increasePose
.
translation
().
norm
();
guessPose
.
rotation
=
fabs
(
increaseRpy
.
z
());
guessPose
.
use_gnss
=
false
;
if
(
!
currMatcher_
->
IsMapLoaded
(
mapPose_
.
translation
()
.
cast
<
double
>
()
)){
currMatcher_
->
loadArea
(
mapPose_
.
translation
()
.
cast
<
double
>
()
);
if
(
!
currMatcher_
->
IsMapLoaded
(
mapPose_
.
translation
())){
currMatcher_
->
loadArea
(
mapPose_
.
translation
());
}
ofs
<<
setprecision
(
16
)
<<
cloudInfo
.
timestamp
<<
", "
<<
mapPose_
.
translation
().
x
()
<<
", "
<<
mapPose_
.
translation
().
y
()
<<
", "
<<
mapPose_
.
translation
().
z
()
<<
endl
;
guessPose
.
precision_type
=
MATCH_PRECISION_FAST
;
float
score
=
currMatcher_
->
MatchPointCloud
(
cloudInfo
.
frame
,
guessPose
,
finalGnssPose
,
mapPose_
);
float
score
=
currMatcher_
->
MatchPointCloud
(
cloudInfo
.
frame
,
guessPose
,
finalGnssPose
,
mapPose_
.
cast
<
float
>
()
);
if
(
score
<
0.45
){
return
false
;
}
guessPose
.
map_point
=
mapPose_
.
cast
<
double
>
();
guessPose
.
precision_type
=
MATCH_PRECISION_HIGH
;
currMatcher_
->
MatchPointCloud
(
cloudInfo
.
frame
,
guessPose
,
finalGnssPose
,
mapPose_
);
currMatcher_
->
MatchPointCloud
(
cloudInfo
.
frame
,
guessPose
,
finalGnssPose
,
mapPose_
.
cast
<
float
>
()
);
return
true
;
}
...
...
@@ -460,10 +460,10 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
LOG
(
INFO
)
<<
"mesh to load: "
<<
streamPath
;
VoxelMapMatcherOption
option
;
option
.
option
.
filter_resolution
=
0.
5
;
option
.
option
.
filter_resolution
=
0.
6
;
option
.
option
.
cloud_range
=
80
;
option
.
fast_option
.
accepted_score
=
0.
4
5
;
option
.
fast_option
.
accepted_low_score
=
0.
5
5
;
option
.
fast_option
.
accepted_score
=
0.
3
5
;
option
.
fast_option
.
accepted_low_score
=
0.
4
5
;
matcher
.
reset
(
new
VoxelMapMatcher
(
option
));
matcher
->
SetMapPath
(
streamPath
);
return
true
;
...
...
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h
View file @
73904574
...
...
@@ -108,7 +108,7 @@ private:
Vector3d
center_
=
Vector3d
::
Zero
();
Isometry3
f
mapPose_
=
Isometry3f
::
Identity
();
Isometry3
d
mapPose_
=
Isometry3d
::
Identity
();
uint32_t
cloudCnt_
=
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