Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding factor which considers the essential matrix and camera calibration as variable #753

Merged
merged 37 commits into from
Jun 21, 2021
Merged
Changes from 1 commit
Commits
Show all changes
37 commits
Select commit Hold shift + click to select a range
8ca7f1f
Adding factor with shared calibration as a variable
ayushbaid Apr 23, 2021
f60e9e9
fixing tests by moving to Cal3_S2
ayushbaid Apr 24, 2021
64ff24b
using fixed size matrix, and adding jacobian in homogeneous conversion
ayushbaid Apr 27, 2021
b00046c
Merge branch 'develop' into feature/essential-mat-with-approx-k
ayushbaid Apr 27, 2021
b0fb6a3
renaming key variable
ayushbaid Apr 27, 2021
bd0838c
fixing docstring
ayushbaid Apr 27, 2021
2cf76da
reverting jacobian computation from homogeneous function
ayushbaid Apr 27, 2021
4572282
adding prior on calibrations
ayushbaid Apr 28, 2021
71b9004
Merge branch 'develop' into feature/essential-mat-with-approx-k
ayushbaid May 25, 2021
bf93f17
Merge branch 'develop' of github.com:borglab/gtsam into feature/essen…
akshay-krishnan Jun 8, 2021
b3601ef
updating tests
ayushbaid Jun 8, 2021
4fbd98d
creating 18 point example
ayushbaid Jun 8, 2021
8a2ce7e
switching to sampson point line error
ayushbaid Jun 9, 2021
0ecd8ab
fixing jacobians and reformatting
ayushbaid Jun 9, 2021
ae69e5f
changing error values in test
ayushbaid Jun 9, 2021
582afe1
Merge branch 'feature/sampson-epipolar-error' into feature/essential-…
ayushbaid Jun 9, 2021
15f8b41
adding jacobians on input points
ayushbaid Jun 9, 2021
e9738c7
adding jacobians on input points
ayushbaid Jun 9, 2021
65211f8
moving to squared sampson error
ayushbaid Jun 9, 2021
2e69d09
Merge branch 'feature/sampson-epipolar-error' into feature/essential-…
ayushbaid Jun 9, 2021
2e8bfd6
using correct jacobian computation for calibration
ayushbaid Jun 10, 2021
91e58f5
fixing unit tests
ayushbaid Jun 10, 2021
bce9050
adding 11 point example for cal3bundler
ayushbaid Jun 10, 2021
620bcf7
fixing test cases
ayushbaid Jun 10, 2021
f1ea57a
Merge branch 'feature/essential-mat-with-approx-k' of github.com:borg…
akshay-krishnan Jun 12, 2021
14f8b8a
removing Sampson error + some tests cleanup
akshay-krishnan Jun 14, 2021
285f041
increasing calibrate() tolerance
akshay-krishnan Jun 14, 2021
373b109
small covariance change
akshay-krishnan Jun 14, 2021
01515d1
formatting changes
akshay-krishnan Jun 14, 2021
c458294
removing duplicate data file
akshay-krishnan Jun 14, 2021
545dfd0
removing failing test and unused data
akshay-krishnan Jun 17, 2021
d9a8111
resolving merge conflict
akshay-krishnan Jun 17, 2021
47f9f30
updating documentation for factor
akshay-krishnan Jun 17, 2021
e3b6c83
updating points name, constexpr
akshay-krishnan Jun 21, 2021
a69f9e5
changing to macro EssenstialMatrixfactor4
akshay-krishnan Jun 21, 2021
119e43a
all jacobian tests for essential matrix use macro
akshay-krishnan Jun 21, 2021
01561bc
formatting example
akshay-krishnan Jun 21, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
updating documentation for factor
  • Loading branch information
akshay-krishnan committed Jun 17, 2021

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
commit 47f9f30b391ccffdcd91db57bd1ceec21ef97a5b
22 changes: 13 additions & 9 deletions gtsam/slam/EssentialMatrixFactor.h
Original file line number Diff line number Diff line change
@@ -294,12 +294,17 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
// EssentialMatrixFactor3

/**
* Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given
* essential matrix and calibration. The calibration is shared between two
* Binary factor that optimizes for E and calibration K using the algebraic
* epipolar error (K^-1 pA)'E (K^-1 pB). The calibration is shared between two
* images.
*
* Note: as correspondences between 2d coordinates can only recover 7 DoF,
*
* Note: As correspondences between 2d coordinates can only recover 7 DoF,
* this factor should always be used with a prior factor on calibration.
* Even with a prior, we can only optimize 2 DoF in the calibration. So the
* prior should have a noise model with very low sigma in the remaining
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the PR, guys. should we give a definition for what range constitutes a "very low" sigma?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this is relative, it depends on the sigma that is not low. There are examples in the unit tests, which are still empirical. Not sure if there is an accurate answer that can be briefly documented.

* dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it
* works only with a strong prior (low sigma noisemodel) on all degrees of
* freedom.
*/
template <class CALIBRATION>
class EssentialMatrixFactor4
@@ -316,15 +321,14 @@ class EssentialMatrixFactor4
public:
/**
* Constructor
* @param keyE Essential Matrix variable key
* @param keyK Calibration variable key
* @param keyE Essential Matrix (from camera B to A) variable key
* @param keyK Calibration variable key (common for both cameras)
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param model noise model is about dot product in ideal, homogeneous
* coordinates
*/
EssentialMatrixFactor4(Key keyE, Key keyK,
const Point2& pA, const Point2& pB,
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model)
: Base(model, keyE, keyK), pA_(pA), pB_(pB) {}

@@ -345,7 +349,7 @@ class EssentialMatrixFactor4
}

/**
* @brief Calculate the algebraic epipolar error p' (K^-1)' E K p.
* @brief Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
*
* @param E essential matrix for key keyE
* @param K calibration (common for both images) for key keyK