12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1896 from borglab/feature/moreComparisons 

More comparisons
release/4.3a0
Frank Dellaert 2024-11-27 17:25:23 -05:00 committed by GitHub
parent 164c35424b baa275bf51
commit 979fb93b98
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 366 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
V_.transpose().matrix();
}
Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
OptionalJacobian<3, 7> H) {
Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
Vector3 line = matrix() * point; // Compute the epipolar line
if (H) {
// Compute the Jacobian if requested
throw std::runtime_error(
"FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
}
return line; // Return the epipolar line
}
void FundamentalMatrix::print(const std::string& s) const {
std::cout << s << matrix() << std::endl;
}
@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
}
Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
OptionalJacobian<3, 7> H) {
Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
Vector3 line = matrix() * point; // Compute the epipolar line
if (H) {
// Compute the Jacobian if requested
throw std::runtime_error(
"SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
}
return line; // Return the epipolar line
}
void SimpleFundamentalMatrix::print(const std::string& s) const {
std::cout << s << " E:\n"
<< E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_

7
gtsam/geometry/FundamentalMatrix.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix {
/// Return the fundamental matrix representation
Matrix3 matrix() const;
/// Computes the epipolar line in a (left) for a given point in b (right)
Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
/// @name Testable
/// @{
/// Print the FundamentalMatrix
@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
/// F = Ka^(-T) * E * Kb^(-1)
Matrix3 matrix() const;
/// Computes the epipolar line in a (left) for a given point in b (right)
Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
/// @name Testable
/// @{
/// Print the SimpleFundamentalMatrix

20
gtsam/sfm/TransferFactor.h
View File

@ -236,6 +236,8 @@ class EssentialTransferFactorK
/**
* @brief Constructor that accepts a vector of point triplets.
*
* @note Calibrations are assumed all different, keys are derived from edges.
*
* @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
* @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
* @param triplets A vector of triplets containing (pa, pb, pc)
@ -251,6 +253,24 @@ class EssentialTransferFactorK
TransferEdges<EM>(edge1, edge2),
triplets_(triplets) {}
/**
* @brief Constructor that accepts a vector of point triplets.
*
* @note Calibrations are assumed all same, using given key `keyK`.
*
* @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
* @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
* @param keyK Calibration key for all views.
* @param triplets A vector of triplets containing (pa, pb, pc)
* @param model An optional SharedNoiseModel
*/
EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK,
const std::vector<Triplet>& triplets,
const SharedNoiseModel& model = nullptr)
: Base(model, edge1, edge2, keyK, keyK, keyK),
TransferEdges<EM>(edge1, edge2),
triplets_(triplets) {}
/// Transfer points pa and pb to view c and evaluate error.
Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
const Matrix3& Ecb, const K& Kb, const Point2& pb,

7
gtsam/sfm/sfm.i
View File

@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const gtsam::noiseModel::Base* model = nullptr);
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const gtsam::noiseModel::Base* model = nullptr);
EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK,
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const gtsam::noiseModel::Base* model = nullptr);
};
#include <gtsam/sfm/ShonanFactor.h>

2
gtsam/sfm/tests/testTransferFactor.cpp
View File

@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) {
}
//*************************************************************************
// Test for EssentialTransferFactor
// Test for EssentialTransferFactorK
TEST(EssentialTransferFactor, Jacobians) {
using namespace fixture;

153
gtsam/slam/EssentialMatrixFactor.h
View File

@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
typedef EssentialMatrixFactor This;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
}
/// vector of errors returns 1D vector
Vector evaluateError(
const EssentialMatrix& E, OptionalMatrixType H) const override {
Vector evaluateError(const EssentialMatrix& E,
OptionalMatrixType H) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
@ -118,7 +117,6 @@ class EssentialMatrixFactor2
typedef EssentialMatrixFactor2 This;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
@ -178,9 +176,9 @@ class EssentialMatrixFactor2
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
Vector evaluateError(const EssentialMatrix& E, const double& d,
OptionalMatrixType DE,
OptionalMatrixType Dd) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
// We then convert to second camera by P2 = 1R2'*(P1-1T2)
@ -241,7 +239,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
Rot3 cRb_; ///< Rotation from body to camera frame
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
Vector evaluateError(const EssentialMatrix& E, const double& d,
OptionalMatrixType DE,
OptionalMatrixType Dd) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
// Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
// does not have the matrix reference version of evaluateError
// Using the pointer version of evaluateError since the Base class
// (EssentialMatrixFactor2) does not have the matrix reference version of
// evaluateError
Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
@ -327,7 +325,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
* 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
* 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
* works only with a strong prior (low sigma noise model) on all degrees of
* freedom.
*/
template <class CALIBRATION>
@ -343,13 +341,12 @@ class EssentialMatrixFactor4
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
// Provide access to the Matrix& version of evaluateError:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param keyE Essential Matrix (from camera B to A) variable key
* @param keyE Essential Matrix aEb 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
@ -357,7 +354,7 @@ class EssentialMatrixFactor4
* coordinates
*/
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model)
const SharedNoiseModel& model = nullptr)
: Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
/// @return a deep copy of this factor
@ -385,32 +382,32 @@ class EssentialMatrixFactor4
* @param H2 optional jacobian of error w.r.t K
* @return * Vector 1D vector of algebraic error
*/
Vector evaluateError(
const EssentialMatrix& E, const CALIBRATION& K,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
OptionalMatrixType HE,
OptionalMatrixType HK) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
// convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vB = EssentialMatrix::Homogeneous(cB);
if (H2) {
if (HK) {
// compute the jacobian of error w.r.t K
// error function f = vA.T * E * vB
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
// where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
// and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
*H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
*HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
}
Vector error(1);
error << E.error(vA, vB, H1);
error << E.error(vA, vB, HE);
return error;
}
@ -420,4 +417,108 @@ class EssentialMatrixFactor4
};
// EssentialMatrixFactor4
/**
* Binary factor that optimizes for E and two calibrations Ka and Kb using the
* algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are
* assumed different for the two images, but if you use the same key for Ka and
* Kb, the sum of the two K Jacobians equals that of the K Jacobian for
* EssentialMatrix4. If you know there is a single global calibration, use
* that factor instead.
*
* Note: see the comment about priors from EssentialMatrixFactor4: even stronger
* caveats about having priors on calibration apply here.
*/
template <class CALIBRATION>
class EssentialMatrixFactor5
: public NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> {
private:
Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> Base;
typedef EssentialMatrixFactor5 This;
static constexpr int DimK = FixedDimension<CALIBRATION>::value;
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param keyE Essential Matrix aEb variable key
* @param keyKa Calibration variable key for camera A
* @param keyKb Calibration variable key for camera B
* @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
*/
EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
const Point2& pB,
const SharedNoiseModel& model = nullptr)
: Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor5 with measurements\n ("
<< pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
<< std::endl;
}
/**
* @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB.
*
* @param E essential matrix for key keyE
* @param Ka calibration for camera A for key keyKa
* @param Kb calibration for camera B for key keyKb
* @param H1 optional jacobian of error w.r.t E
* @param H2 optional jacobian of error w.r.t Ka
* @param H3 optional jacobian of error w.r.t Kb
* @return * Vector 1D vector of algebraic error
*/
Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka,
const CALIBRATION& Kb, OptionalMatrixType HE,
OptionalMatrixType HKa,
OptionalMatrixType HKb) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_Ka; // dcA/dKa
JacobianCalibration cB_H_Kb; // dcB/dKb
Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
// Convert to homogeneous coordinates.
Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vB = EssentialMatrix::Homogeneous(cB);
if (HKa) {
// Compute the jacobian of error w.r.t Ka.
*HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
}
if (HKb) {
// Compute the jacobian of error w.r.t Kb.
*HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
}
Vector error(1);
error << E.error(vA, vB, HE);
return error;
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor5
} // namespace gtsam

50
gtsam/slam/slam.i
View File

@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Unified;
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
GeneralSFMFactor2(const gtsam::Point2& measured,
@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
#include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
const gtsam::Point2& pB,
const gtsam::noiseModel::Base* noiseModel);
EssentialMatrixFactor(size_t key,
const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* model);
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const;
};
virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor {
EssentialMatrixFactor2(size_t key1, size_t key2,
const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* model);
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
};
virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 {
EssentialMatrixFactor3(size_t key1, size_t key2,
const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model);
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
};
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor {
EssentialMatrixFactor4(size_t keyE, size_t keyK,
const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* model = nullptr);
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const;
};
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor {
EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb,
const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* model = nullptr);
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E,
const CALIBRATION& Ka, const CALIBRATION& Kb) const;
};
#include <gtsam/slam/EssentialMatrixConstraint.h>
virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,

70
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -14,8 +14,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
using namespace std::placeholders;
@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
Key key(1);
for (size_t i = 0; i < 5; i++) {
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i),
std::placeholders::_2);
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression
@ -116,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
// Check evaluation
Vector expected(1);
expected << 0;
vector<Matrix> Hactual(1);
Vector actual = factor.unwhitenedError(values, Hactual);
vector<Matrix> actualH(1);
Vector actual = factor.unwhitenedError(values, actualH);
EXPECT(assert_equal(expected, actual, 1e-7));
}
}
@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
Key key(1);
for (size_t i = 0; i < 5; i++) {
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i),
std::placeholders::_2);
std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;
Expression<Rot3> R_(key);
Expression<Unit3> d_(trueDirection);
@ -149,8 +151,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
// Check evaluation
Vector expected(1);
expected << 0;
vector<Matrix> Hactual(1);
Vector actual = factor.unwhitenedError(values, Hactual);
vector<Matrix> actualH(1);
Vector actual = factor.unwhitenedError(values, actualH);
EXPECT(assert_equal(expected, actual, 1e-7));
}
}
@ -211,9 +213,9 @@ TEST(EssentialMatrixFactor2, factor) {
const Point2 pi = PinholeBase::Project(P2);
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
Matrix actualH1, actualH2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2);
EXPECT(assert_equal(expected, actual, 1e-7));
Values val;
@ -276,9 +278,9 @@ TEST(EssentialMatrixFactor3, factor) {
const Point2 pi = camera2.project(P1);
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
Matrix actualH1, actualH2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2);
EXPECT(assert_equal(expected, actual, 1e-7));
Values val;
@ -529,6 +531,48 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
1e-6);
}
//*************************************************************************
TEST(EssentialMatrixFactor5, factor) {
Key keyE(1), keyKa(2), keyKb(3);
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor5<Cal3_S2> factor(keyE, keyKa, keyKb, pA(i), pB(i),
model1);
// Check evaluation
Vector1 expected;
expected << 0;
Vector actual = factor.evaluateError(trueE, trueK, trueK);
EXPECT(assert_equal(expected, actual, 1e-7));
Values truth;
truth.insert(keyE, trueE);
truth.insert(keyKa, trueK);
truth.insert(keyKb, trueK);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
}
}
//*************************************************************************
// Test that if we use the same keys for Ka and Kb the sum of the two K
// Jacobians equals that of the single K Jacobian for EssentialMatrix4
TEST(EssentialMatrixFactor5, SameKeys) {
Key keyE(1), keyK(2);
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor4<Cal3_S2> factor4(keyE, keyK, pA(i), pB(i), model1);
EssentialMatrixFactor5<Cal3_S2> factor5(keyE, keyK, keyK, pA(i), pB(i),
model1);
Values truth;
truth.insert(keyE, trueE);
truth.insert(keyK, trueK);
// Check Jacobians
Matrix H0, H1, H2;
factor4.evaluateError(trueE, trueK, nullptr, &H0);
factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2);
EXPECT(assert_equal(H0, H1 + H2, 1e-9));
}
}
} // namespace example1
//*************************************************************************

121
python/gtsam/examples/ViewGraphComparison.py
View File

@ -32,7 +32,7 @@ from gtsam import (
K = gtsam.symbol_shorthand.K
# Methods to compare
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"]
# Formatter function for printing keys
@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std):
return measurements
# Function to compute ground truth matrices
def compute_ground_truth(method, poses, cal):
"""Function to compute ground truth edge variables."""
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
F1 = FundamentalMatrix(cal.K(), E1, cal.K())
@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal):
SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
return SF1, SF2
elif method == "Essential+Ks" or method == "Calibrated":
return E1, E2
else:
raise ValueError(f"Unknown method {method}")
return E1, E2
def build_factor_graph(method, num_cameras, measurements, cal):
"""build the factor graph"""
graph = NonlinearFactorGraph()
# Determine the FactorClass based on the method
if method == "Fundamental":
FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "SimpleF":
FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
elif method == "Essential+Ks":
elif method in ["Essential+Ks", "Essential+K"]:
FactorClass = gtsam.EssentialTransferFactorKCal3f
# add priors on all calibrations:
for i in range(num_cameras):
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
graph.addPriorCal3f(K(i), cal, model)
elif method == "Binary+K":
FactorClass = gtsam.EssentialMatrixFactor4Cal3f
elif method == "Binary+Ks":
FactorClass = gtsam.EssentialMatrixFactor5Cal3f
elif method == "Calibrated":
FactorClass = gtsam.EssentialTransferFactorCal3f
# No priors on calibration needed
else:
raise ValueError(f"Unknown method {method}")
# Add priors on calibrations if necessary
if method in ["Essential+Ks", "Binary+Ks"]:
for i in range(num_cameras):
model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0)
graph.addPriorCal3f(K(i), cal, model)
elif method in ["Essential+K", "Binary+K"]:
model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0)
graph.addPriorCal3f(K(0), cal, model)
z = measurements # shorthand
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
# Vectors to collect tuples for each factor
tuples1 = []
tuples2 = []
tuples3 = []
# Collect data for the three factors
for j in range(len(measurements[0])):
tuples1.append((z[a][j], z[b][j], z[c][j]))
tuples2.append((z[a][j], z[c][j], z[b][j]))
tuples3.append((z[c][j], z[b][j], z[a][j]))
# Add transfer factors between views a, b, and c.
if method in ["Calibrated"]:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
if method in ["Binary+Ks", "Binary+K"]:
# Add binary Essential Matrix factors
ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key()
for j in range(len(measurements[0])):
if method == "Binary+Ks":
graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j]))
graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j]))
else: # Binary+K
graph.add(FactorClass(ab, K(0), z[a][j], z[b][j]))
graph.add(FactorClass(ac, K(0), z[a][j], z[c][j]))
else:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
# Add transfer factors between views a, b, and c
# Vectors to collect tuples for each factor
tuples1 = []
tuples2 = []
tuples3 = []
for j in range(len(measurements[0])):
tuples1.append((z[a][j], z[b][j], z[c][j]))
tuples2.append((z[a][j], z[c][j], z[b][j]))
tuples3.append((z[c][j], z[b][j], z[a][j]))
# Add transfer factors between views a, b, and c.
if method in ["Calibrated"]:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
elif method == "Essential+K":
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3))
else:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
return graph
@ -159,22 +181,25 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
initialEstimate.insert(EdgeKey(a, b).key(), F1)
initialEstimate.insert(EdgeKey(a, c).key(), F2)
total_dimension += F1.dim() + F2.dim()
elif method in ["Essential+Ks", "Calibrated"]:
elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
E1, E2 = ground_truth
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
b = (a + 1) % num_cameras
c = (a + 2) % num_cameras
# initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1))))
# initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1))))
initialEstimate.insert(EdgeKey(a, b).key(), E1)
initialEstimate.insert(EdgeKey(a, c).key(), E2)
total_dimension += E1.dim() + E2.dim()
else:
raise ValueError(f"Unknown method {method}")
if method == "Essential+Ks":
# Insert initial calibrations
# Insert initial calibrations
if method in ["Essential+Ks", "Binary+Ks"]:
for i in range(num_cameras):
initialEstimate.insert(K(i), cal)
total_dimension += cal.dim()
elif method in ["Essential+K", "Binary+K"]:
initialEstimate.insert(K(0), cal)
total_dimension += cal.dim()
print(f"Total dimension of the problem: {total_dimension}")
return initialEstimate
@ -183,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
def optimize(graph, initialEstimate, method):
"""optimize the graph"""
params = LevenbergMarquardtParams()
params.setlambdaInitial(1e10) # Initialize lambda to a high value
params.setlambdaUpperBound(1e10)
if method not in ["Calibrated", "Binary+K", "Binary+Ks"]:
params.setlambdaInitial(1e10) # Initialize lambda to a high value
params.setlambdaUpperBound(1e10)
# params.setAbsoluteErrorTol(0.1)
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
@ -205,7 +231,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
key_ab = EdgeKey(a, b).key()
key_ac = EdgeKey(a, c).key()
if method in ["Essential+Ks", "Calibrated"]:
if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
E_est_ab = result.atEssentialMatrix(key_ab)
E_est_ac = result.atEssentialMatrix(key_ac)
@ -218,15 +244,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
F_est_ab = FundamentalMatrix(SF_est_ab)
F_est_ac = FundamentalMatrix(SF_est_ac)
elif method == "Essential+Ks":
# Retrieve calibrations from result:
elif method in ["Essential+Ks", "Binary+Ks"]:
# Retrieve calibrations from result for each camera
cal_a = result.atCal3f(K(a))
cal_b = result.atCal3f(K(b))
cal_c = result.atCal3f(K(c))
# Convert estimated EssentialMatrices to FundamentalMatrices
F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
elif method in ["Essential+K", "Binary+K"]:
# Use single shared calibration
cal_shared = result.atCal3f(K(0))
F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K())
F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K())
elif method == "Calibrated":
# Use ground truth calibration
F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())
@ -347,6 +376,8 @@ def main():
# Compute final error
final_error = graph.error(result)
if method in ["Binary+K", "Binary+Ks"]:
final_error *= cal.f() * cal.f()
# Store results
results[method]["distances"].extend(distances)