Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
I
imu
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
mobile_robotics
wolf_projects
wolf_lib
plugins
imu
Commits
8fbb3669
Commit
8fbb3669
authored
6 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Plain Diff
Merge branch 'isometry' into 'devel'
replace Affine --> Isometry See merge request
!2
parents
af9212ab
837b3ce1
No related branches found
No related tags found
3 merge requests
!39
release after RAL
,
!38
After 2nd RAL submission
,
!2
replace Affine --> Isometry
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/processor/processor_tracker_landmark_apriltag.cpp
+14
-14
14 additions, 14 deletions
src/processor/processor_tracker_landmark_apriltag.cpp
with
14 additions
and
14 deletions
src/processor/processor_tracker_landmark_apriltag.cpp
+
14
−
14
View file @
8fbb3669
...
@@ -145,12 +145,12 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
...
@@ -145,12 +145,12 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
int
tag_id
=
det
->
id
;
int
tag_id
=
det
->
id
;
Scalar
tag_width
=
getTagWidth
(
tag_id
);
// tag width in meters
Scalar
tag_width
=
getTagWidth
(
tag_id
);
// tag width in meters
Eigen
::
Affine
3ds
c_M_t
;
Eigen
::
Isometry
3ds
c_M_t
;
bool
use_rotation
=
true
;
bool
use_rotation
=
true
;
//////////////////
//////////////////
// IPPE (Infinitesimal Plane-based Pose Estimation)
// IPPE (Infinitesimal Plane-based Pose Estimation)
//////////////////
//////////////////
Eigen
::
Affine
3ds
M_ippe1
,
M_ippe2
,
M_april
,
M_PnP
;
Eigen
::
Isometry
3ds
M_ippe1
,
M_ippe2
,
M_april
,
M_PnP
;
Scalar
rep_error1
;
Scalar
rep_error1
;
Scalar
rep_error2
;
Scalar
rep_error2
;
ippePoseEstimation
(
det
,
cv_K_
,
tag_width
,
M_ippe1
,
rep_error1
,
M_ippe2
,
rep_error2
);
ippePoseEstimation
(
det
,
cv_K_
,
tag_width
,
M_ippe1
,
rep_error1
,
M_ippe2
,
rep_error2
);
...
@@ -184,9 +184,9 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
...
@@ -184,9 +184,9 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
}
}
void
ProcessorTrackerLandmarkApriltag
::
ippePoseEstimation
(
apriltag_detection_t
*
_det
,
cv
::
Mat_
<
Scalar
>
_K
,
double
_tag_width
,
void
ProcessorTrackerLandmarkApriltag
::
ippePoseEstimation
(
apriltag_detection_t
*
_det
,
cv
::
Mat_
<
Scalar
>
_K
,
double
_tag_width
,
Eigen
::
Affine
3d
&
_M1
,
Eigen
::
Isometry
3d
&
_M1
,
double
&
_rep_error1
,
double
&
_rep_error1
,
Eigen
::
Affine
3d
&
_M2
,
Eigen
::
Isometry
3d
&
_M2
,
double
&
_rep_error2
){
double
&
_rep_error2
){
// get corners from det
// get corners from det
...
@@ -258,20 +258,20 @@ LandmarkBasePtr ProcessorTrackerLandmarkApriltag::createLandmark(FeatureBasePtr
...
@@ -258,20 +258,20 @@ LandmarkBasePtr ProcessorTrackerLandmarkApriltag::createLandmark(FeatureBasePtr
// world to rob
// world to rob
Vector3s
pos
=
getLast
()
->
getFrame
()
->
getP
()
->
getState
();
Vector3s
pos
=
getLast
()
->
getFrame
()
->
getP
()
->
getState
();
Quaternions
quat
(
getLast
()
->
getFrame
()
->
getO
()
->
getState
().
data
());
Quaternions
quat
(
getLast
()
->
getFrame
()
->
getO
()
->
getState
().
data
());
Eigen
::
Affine
3ds
w_M_r
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
.
head
(
3
))
*
quat
;
Eigen
::
Isometry
3ds
w_M_r
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
.
head
(
3
))
*
quat
;
// rob to camera
// rob to camera
pos
=
getSensor
()
->
getP
()
->
getState
();
pos
=
getSensor
()
->
getP
()
->
getState
();
quat
.
coeffs
()
=
getSensor
()
->
getO
()
->
getState
();
quat
.
coeffs
()
=
getSensor
()
->
getO
()
->
getState
();
Eigen
::
Affine
3ds
r_M_c
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
.
head
(
3
))
*
quat
;
Eigen
::
Isometry
3ds
r_M_c
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
.
head
(
3
))
*
quat
;
// camera to lmk (tag)
// camera to lmk (tag)
pos
=
_feature_ptr
->
getMeasurement
().
head
(
3
);
pos
=
_feature_ptr
->
getMeasurement
().
head
(
3
);
quat
.
coeffs
()
=
_feature_ptr
->
getMeasurement
().
tail
(
4
);
quat
.
coeffs
()
=
_feature_ptr
->
getMeasurement
().
tail
(
4
);
Eigen
::
Affine
3ds
c_M_t
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
)
*
quat
;
Eigen
::
Isometry
3ds
c_M_t
=
Eigen
::
Translation
<
Scalar
,
3
>
(
pos
)
*
quat
;
// world to lmk (tag)
// world to lmk (tag)
Eigen
::
Affine
3ds
w_M_t
=
w_M_r
*
r_M_c
*
c_M_t
;
Eigen
::
Isometry
3ds
w_M_t
=
w_M_r
*
r_M_c
*
c_M_t
;
// make 7-vector for lmk (tag) pose
// make 7-vector for lmk (tag) pose
pos
=
w_M_t
.
translation
();
pos
=
w_M_t
.
translation
();
...
@@ -551,7 +551,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
...
@@ -551,7 +551,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// Retrieve camera extrinsics
// Retrieve camera extrinsics
Eigen
::
Quaternions
last_q_cam
(
getSensor
()
->
getO
()
->
getState
().
data
());
Eigen
::
Quaternions
last_q_cam
(
getSensor
()
->
getO
()
->
getState
().
data
());
Eigen
::
Affine
3ds
last_M_cam
=
Eigen
::
Translation3ds
(
getSensor
()
->
getP
()
->
getState
())
*
last_q_cam
;
Eigen
::
Isometry
3ds
last_M_cam
=
Eigen
::
Translation3ds
(
getSensor
()
->
getP
()
->
getState
())
*
last_q_cam
;
// get features list of KF linked to origin capture and last capture
// get features list of KF linked to origin capture and last capture
FeatureBasePtrList
ori_feature_list
=
getOrigin
()
->
getFeatureList
();
FeatureBasePtrList
ori_feature_list
=
getOrigin
()
->
getFeatureList
();
...
@@ -593,10 +593,10 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
...
@@ -593,10 +593,10 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// Retrieve cam to landmark transform
// Retrieve cam to landmark transform
Eigen
::
Vector7s
cam_pose_lmk
=
best_feature
->
getMeasurement
();
Eigen
::
Vector7s
cam_pose_lmk
=
best_feature
->
getMeasurement
();
Eigen
::
Quaternions
cam_q_lmk
(
cam_pose_lmk
.
segment
<
4
>
(
3
).
data
());
Eigen
::
Quaternions
cam_q_lmk
(
cam_pose_lmk
.
segment
<
4
>
(
3
).
data
());
Eigen
::
Affine
3ds
cam_M_lmk
=
Eigen
::
Translation3ds
(
cam_pose_lmk
.
head
(
3
))
*
cam_q_lmk
;
Eigen
::
Isometry
3ds
cam_M_lmk
=
Eigen
::
Translation3ds
(
cam_pose_lmk
.
head
(
3
))
*
cam_q_lmk
;
// Get corresponding landmarks in origin/last landmark list
// Get corresponding landmarks in origin/last landmark list
Eigen
::
Affine
3ds
w_M_lmk
;
Eigen
::
Isometry
3ds
w_M_lmk
;
LandmarkBasePtrList
lmk_list
=
getProblem
()
->
getMap
()
->
getLandmarkList
();
LandmarkBasePtrList
lmk_list
=
getProblem
()
->
getMap
()
->
getLandmarkList
();
// Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers)
// Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers)
for
(
std
::
list
<
LandmarkBasePtr
>::
reverse_iterator
it_lmk
=
lmk_list
.
rbegin
();
it_lmk
!=
lmk_list
.
rend
();
++
it_lmk
){
for
(
std
::
list
<
LandmarkBasePtr
>::
reverse_iterator
it_lmk
=
lmk_list
.
rbegin
();
it_lmk
!=
lmk_list
.
rend
();
++
it_lmk
){
...
@@ -614,7 +614,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
...
@@ -614,7 +614,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
}
}
// Compute last frame estimate
// Compute last frame estimate
Eigen
::
Affine
3ds
w_M_last
=
w_M_lmk
*
(
last_M_cam
*
cam_M_lmk
).
inverse
();
Eigen
::
Isometry
3ds
w_M_last
=
w_M_lmk
*
(
last_M_cam
*
cam_M_lmk
).
inverse
();
// Use the w_M_last to overide last key frame state estimation and keyframe estimation
// Use the w_M_last to overide last key frame state estimation and keyframe estimation
Eigen
::
Vector3s
pos_last
=
w_M_last
.
translation
();
Eigen
::
Vector3s
pos_last
=
w_M_last
.
translation
();
...
@@ -633,8 +633,8 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
...
@@ -633,8 +633,8 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
Eigen
::
Vector7s
cam_pose_lmk
=
new_feature_last
->
getMeasurement
();
Eigen
::
Vector7s
cam_pose_lmk
=
new_feature_last
->
getMeasurement
();
Eigen
::
Quaternions
cam_q_lmk
(
cam_pose_lmk
.
segment
<
4
>
(
3
).
data
());
Eigen
::
Quaternions
cam_q_lmk
(
cam_pose_lmk
.
segment
<
4
>
(
3
).
data
());
Eigen
::
Affine
3ds
cam_M_lmk_new
=
Eigen
::
Translation3ds
(
cam_pose_lmk
.
head
(
3
))
*
cam_q_lmk
;
Eigen
::
Isometry
3ds
cam_M_lmk_new
=
Eigen
::
Translation3ds
(
cam_pose_lmk
.
head
(
3
))
*
cam_q_lmk
;
Eigen
::
Affine
3ds
w_M_lmk
=
w_M_last
*
last_M_cam
*
cam_M_lmk_new
;
Eigen
::
Isometry
3ds
w_M_lmk
=
w_M_last
*
last_M_cam
*
cam_M_lmk_new
;
for
(
auto
it_lmk
=
new_landmarks_
.
begin
();
it_lmk
!=
new_landmarks_
.
end
();
++
it_lmk
){
for
(
auto
it_lmk
=
new_landmarks_
.
begin
();
it_lmk
!=
new_landmarks_
.
end
();
++
it_lmk
){
LandmarkApriltagPtr
lmk_ptr
=
std
::
dynamic_pointer_cast
<
LandmarkApriltag
>
(
*
it_lmk
);
LandmarkApriltagPtr
lmk_ptr
=
std
::
dynamic_pointer_cast
<
LandmarkApriltag
>
(
*
it_lmk
);
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment