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

reverting jacobian computation from homogeneous function

release/4.3a0
Ayush Baid 2021-04-27 00:44:15 -04:00
parent bd0838c0c9
commit 2cf76daf3c
3 changed files with 10 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/EssentialMatrix.h
View File

@ -7,10 +7,10 @@
#pragma once #pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd> #include <iosfwd>
#include <string> #include <string>
@ -31,11 +31,7 @@ class EssentialMatrix {
public: public:
/// Static function to convert Point2 to homogeneous coordinates /// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p, static Vector3 Homogeneous(const Point2& p) {
OptionalJacobian<3, 2> H = boost::none) {
if (H) {
H->setIdentity();
}
return Vector3(p.x(), p.y(), 1); return Vector3(p.x(), p.y(), 1);
} }

12
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -241,18 +241,6 @@ TEST (EssentialMatrix, epipoles) {
EXPECT(assert_equal(e2, E.epipole_b())); EXPECT(assert_equal(e2, E.epipole_b()));
} }
//*************************************************************************
TEST(EssentialMatrix, Homogeneous) {
Point2 input(5.0, 1.3);
Vector3 expected(5.0, 1.3, 1.0);
Matrix32 expectedH;
expectedH << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0;
Matrix32 actualH;
Vector3 actual = EssentialMatrix::Homogeneous(input, actualH);
EXPECT(assert_equal(actual, expected));
EXPECT(assert_equal(actualH, expectedH));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

14
gtsam/slam/EssentialMatrixFactor.h
View File

@ -354,17 +354,15 @@ class EssentialMatrixFactor4
const EssentialMatrix& E, const CALIBRATION& K, const EssentialMatrix& E, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override { boost::optional<Matrix&> H2 = boost::none) const override {
Vector error(1);
// 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); Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0);
// Homogeneous the coordinates // convert to homogeneous coordinates
Matrix32 vA_H_cA, vB_H_cB; Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vA = EssentialMatrix::Homogeneous(cA, H2 ? &vA_H_cA : 0); Vector3 vB = EssentialMatrix::Homogeneous(cB);
Vector3 vB = EssentialMatrix::Homogeneous(cB, H2 ? &vB_H_cB : 0);
if (H2) { if (H2) {
// compute the jacobian of error w.r.t K // compute the jacobian of error w.r.t K
@ -372,10 +370,12 @@ class EssentialMatrixFactor4
// 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
*H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K + // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
vA.transpose() * E.matrix() * vB_H_cB * cB_H_K; *H2 = 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, H1);
return error; return error;