Combine Pose SE(3) and Similarity Sim(3) Measurements
342 views
Skip to first unread message
Thomas Eberhard
Dec 9, 2021, 12:28:02 PM12/9/21
to gtsam users
Hello everybody,
thanks for inviting me into the group.
I have drone data that has an approximately correct Pose / SE(3) from a kalman-filtered GPS-signal.
In addition I am able to track visual features over time, e.g. by using ORB Features.
I know how to obtain an Essential Matrix from this and hence a Sim(3) transformation between my poses.
I believe that although the obtained Sim(3) has one degree of freedom more than the related SE(3) Poses from the Kalmanfilter they should be useful for constraining the possible poses.
Is there a way to jointly optimize for this or should I instead use my obtained features and use them as Landmarks?
I am reluctant to do so because it will not work if I obtain my Essential Matrix from a direct method, instead of an indirect method like ORB.
In general I am interested in combining a visual method that outputs a Sim(3) with an precomputed Pose Estimation or an IMUFactor but I am not able to interfere from the documentation how to approach this.
Is the idea itself wrong or is there another reason why I can't find much references to this?
Could you maybe kindly help me by giving an explanation or pointing be to code that is doing something in this regard?
Best,
Thomas
thanks for inviting me into the group.
I have drone data that has an approximately correct Pose / SE(3) from a kalman-filtered GPS-signal.
In addition I am able to track visual features over time, e.g. by using ORB Features.
I know how to obtain an Essential Matrix from this and hence a Sim(3) transformation between my poses.
I believe that although the obtained Sim(3) has one degree of freedom more than the related SE(3) Poses from the Kalmanfilter they should be useful for constraining the possible poses.
Is there a way to jointly optimize for this or should I instead use my obtained features and use them as Landmarks?
I am reluctant to do so because it will not work if I obtain my Essential Matrix from a direct method, instead of an indirect method like ORB.
In general I am interested in combining a visual method that outputs a Sim(3) with an precomputed Pose Estimation or an IMUFactor but I am not able to interfere from the documentation how to approach this.
Is the idea itself wrong or is there another reason why I can't find much references to this?
Could you maybe kindly help me by giving an explanation or pointing be to code that is doing something in this regard?
Best,
Thomas
J Zhang
Dec 10, 2021, 1:28:02 PM12/10/21
to gtsam users
Assume X is your pose node, does a factor graph like this works for you?
prior - X0 -(SIM3) - X1 - (Sim3) - X2 --
| | |
SE(3). SE(3). SE(3)
So your Sim(3) becomes the betweenfactor constraint and KF siginal becomes the prior factor for the pose.
Frank Dellaert
Dec 10, 2021, 2:30:37 PM12/10/21
to gtsam users
Take a look at EssentialMatrixConstraint.h and its tests...
Thomas Eberhard
Dec 11, 2021, 10:15:31 AM12/11/21
to gtsam users
Thanks for your swift answer.
From what I understand the factor in X0 - (SIM3) - X1 is represented by EssentialMatrixConstraint.h
I confused it with EssentialMatrixFactor.h which can be used to compute an essential matrix if I understand correctly.
I am currently using the python API where I seem to have no access to EssentialMatrixConstraint code.
I would like to implement and give a merge request for this to be accessible in python.
Is there a reference I can look at to achieve this?
From what I understand the factor in X0 - (SIM3) - X1 is represented by EssentialMatrixConstraint.h
I confused it with EssentialMatrixFactor.h which can be used to compute an essential matrix if I understand correctly.
I am currently using the python API where I seem to have no access to EssentialMatrixConstraint code.
I would like to implement and give a merge request for this to be accessible in python.
Is there a reference I can look at to achieve this?
Frank Dellaert
Dec 11, 2021, 1:27:37 PM12/11/21
to gtsam users
It's easier to do than to explain. I did a quick PR here: https://github.com/borglab/gtsam/pull/962
You can pull develop after it merges or even merge in the branch, if you want.
Best!
Frank
Reply all
Reply to author
Forward
0 new messages