Go to the documentation of this file.
16 using namespace std::placeholders;
18 using namespace gtsam;
45 return EssentialMatrix::FromRotationAndDirection(
R,
trueDirection);
53 return EssentialMatrix::FromRotationAndDirection(
trueRotation,
t);
72 Vector actual =
E.localCoordinates(
E);
86 d << 0.1, 0.2, 0.3, 0, 0, 0;
116 d << 0.1, 0.2, 0.3, 0.4, 0.5;
118 Vector actual =
e.localCoordinates(hx);
124 return E.transformTo(
point);
129 * Rot3::Roll(
M_PI / 6.0);
130 Point3 aTb2(19.2, 3.7, 5.9);
135 E.transformTo(
P, actH1, actH2);
144 return E.rotate(
cRb);
168 }
catch (exception&
e) {
182 return EssentialMatrix::FromPose3(
pose);
184 Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
fn,
pose);
197 return EssentialMatrix::FromPose3(
pose);
199 Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
fn,
pose);
239 Unit3 e1(-
U(0, 2), -
U(1, 2), -
U(2, 2));
240 Unit3 actual =
E.epipole_a();
250 Unit3 e2(
V(0, 2),
V(1, 2),
V(2, 2));
static int runAllTests(TestResult &result)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
EssentialMatrix rotate_(const EssentialMatrix &E, const Rot3 &cRb)
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
Point3 transform_to_(const EssentialMatrix &E, const Point3 &point)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Unit3 trueDirection(trueTranslation)
Calibrated camera for which only pose is unknown.
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
static std::stringstream ss
gtsam::Point3 bX(1, 0, 0)
Some functions to compute numerical derivatives.
gtsam::Point3 bY(0, 1, 0)
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
GTSAM_EXPORT EssentialMatrix rotate(const Rot3 &cRb, OptionalJacobian< 5, 5 > HE={}, OptionalJacobian< 5, 3 > HR={}) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Point3 trueTranslation(0.1, 0, 0)
TEST(EssentialMatrix, equality)
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Rot3 inverse() const
inverse of a rotation
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
bool equality(const Errors &actual, const Errors &expected, double tol)
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
**
gtsam::Point3 bZ(0, 0, 1)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
EssentialMatrix trueE(trueRotation, trueDirection)
Represents a 3D point on a unit sphere.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
P rotate(const T &r, const P &pt)
Rot2 R(Rot2::fromAngle(0.1))
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:51