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

EssentialMatrixFactor5

release/4.3a0
Frank Dellaert 2024-10-30 14:54:57 -07:00
parent eca2bb5d8a
commit 45fc039d07
2 changed files with 150 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
gtsam/slam/EssentialMatrixFactor.h
View File

@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
typedef EssentialMatrixFactor This; typedef EssentialMatrixFactor This;
public: public:
// Provide access to the Matrix& version of evaluateError: // Provide access to the Matrix& version of evaluateError:
using Base::evaluateError; using Base::evaluateError;
@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
} }
/// vector of errors returns 1D vector /// vector of errors returns 1D vector
Vector evaluateError( Vector evaluateError(const EssentialMatrix& E,
const EssentialMatrix& E, OptionalMatrixType H) const override { OptionalMatrixType H) const override {
Vector error(1); Vector error(1);
error << E.error(vA_, vB_, H); error << E.error(vA_, vB_, H);
return error; return error;
@ -118,7 +117,6 @@ class EssentialMatrixFactor2
typedef EssentialMatrixFactor2 This; typedef EssentialMatrixFactor2 This;
public: public:
// Provide access to the Matrix& version of evaluateError: // Provide access to the Matrix& version of evaluateError:
using Base::evaluateError; using Base::evaluateError;
@ -178,9 +176,9 @@ class EssentialMatrixFactor2
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError( Vector evaluateError(const EssentialMatrix& E, const double& d,
const EssentialMatrix& E, const double& d, OptionalMatrixType DE,
OptionalMatrixType DE, OptionalMatrixType Dd) const override { OptionalMatrixType Dd) const override {
// We have point x,y in image 1 // 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 // 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) // 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 Rot3 cRb_; ///< Rotation from body to camera frame
public: public:
// Provide access to the Matrix& version of evaluateError: // Provide access to the Matrix& version of evaluateError:
using Base::evaluateError; using Base::evaluateError;
@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError( Vector evaluateError(const EssentialMatrix& E, const double& d,
const EssentialMatrix& E, const double& d, OptionalMatrixType DE,
OptionalMatrixType DE, OptionalMatrixType Dd) const override { OptionalMatrixType Dd) const override {
if (!DE) { if (!DE) {
// Convert E from body to camera frame // Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E; EssentialMatrix cameraE = cRb_ * E;
@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
// Version with derivatives // Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
// Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2) // Using the pointer version of evaluateError since the Base class
// does not have the matrix reference version of evaluateError // (EssentialMatrixFactor2) does not have the matrix reference version of
// evaluateError
Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd); Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e; 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 * 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 * 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 * 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. * freedom.
*/ */
template <class CALIBRATION> template <class CALIBRATION>
@ -343,13 +341,12 @@ class EssentialMatrixFactor4
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration; typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public: public:
// Provide access to the Matrix& version of evaluateError:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError; using Base::evaluateError;
/** /**
* Constructor * 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 keyK Calibration variable key (common for both cameras)
* @param pA point in first camera, in pixel coordinates * @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates
@ -385,32 +382,32 @@ class EssentialMatrixFactor4
* @param H2 optional jacobian of error w.r.t K * @param H2 optional jacobian of error w.r.t K
* @return * Vector 1D vector of algebraic error * @return * Vector 1D vector of algebraic error
*/ */
Vector evaluateError( Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
const EssentialMatrix& E, const CALIBRATION& K, OptionalMatrixType HE,
OptionalMatrixType H1, OptionalMatrixType H2) const override { OptionalMatrixType HK) const override {
// converting from pixel coordinates to normalized coordinates cA and cB // converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
// convert to homogeneous coordinates // convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vB = EssentialMatrix::Homogeneous(cB); Vector3 vB = EssentialMatrix::Homogeneous(cB);
if (H2) { if (HK) {
// compute the jacobian of error w.r.t K // compute the jacobian of error w.r.t K
// error function f = vA.T * E * vB // error function f = vA.T * E * vB
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // 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 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
// and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] // 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; vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
} }
Vector error(1); Vector error(1);
error << E.error(vA, vB, H1); error << E.error(vA, vB, HE);
return error; return error;
} }
@ -420,4 +417,104 @@ class EssentialMatrixFactor4
}; };
// 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. Don'tt use this factor with same
* calibration unknown, as Jacobians will be incorrect...
*
* Note: even stronger caveats about having priors on calibration apply.
*/
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)
: 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 } // namespace gtsam

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

@ -14,8 +14,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressionTesting.h> #include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h> #include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/sfm/SfmData.h> #include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
using namespace std::placeholders; using namespace std::placeholders;
@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
Key key(1); Key key(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f = 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<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression Expression<double> expr(f, E_); // unary expression
@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
Key key(1); Key key(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f = 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 &, std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 3>, OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)> OptionalJacobian<5, 2>)>
g; g;
Expression<Rot3> R_(key); Expression<Rot3> R_(key);
Expression<Unit3> d_(trueDirection); Expression<Unit3> d_(trueDirection);
@ -529,6 +531,27 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
1e-6); 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);
}
}
} // namespace example1 } // namespace example1
//************************************************************************* //*************************************************************************