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

address comments

release/4.3a0
Frank Dellaert 2024-11-26 09:53:28 -08:00
parent 5f667b4425
commit baa275bf51
2 changed files with 47 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/EssentialMatrixFactor.h
View File

@ -420,10 +420,13 @@ class 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...
* 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: even stronger caveats about having priors on calibration apply.
* Note: see the comment about priors from EssentialMatrixFactor4: even stronger
* caveats about having priors on calibration apply here.
*/
template <class CALIBRATION>
class EssentialMatrixFactor5
@ -452,7 +455,8 @@ class EssentialMatrixFactor5
* coordinates
*/
EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
const Point2& pB, const SharedNoiseModel& model = nullptr)
const Point2& pB,
const SharedNoiseModel& model = nullptr)
: Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
/// @return a deep copy of this factor
@ -492,17 +496,17 @@ class EssentialMatrixFactor5
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
// 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
// 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
// Compute the jacobian of error w.r.t Kb.
*HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
}

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

@ -117,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));
}
}
@ -151,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));
}
}
@ -213,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;
@ -278,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;
@ -552,6 +552,27 @@ TEST(EssentialMatrixFactor5, factor) {
}
}
//*************************************************************************
// 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
//*************************************************************************