Go to the documentation of this file.
15 using namespace gtsam;
17 static const double tol = 1
e-5;
20 TEST( testPose3Upright, basics ) {
43 actual2(1.0, 2.0, 3.0, 0.1), actual3(4.0,-7.0, 3.0, 0.3);
56 TEST( testPose3Upright, conversions ) {
67 TEST( testPose3Upright, manifold ) {
75 Vector delta12 = (
Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12;
84 TEST( testPose3Upright, lie ) {
99 Matrix actualH1, actualH2, numericH1, numericH2;
114 Matrix actualH1, actualH2, numericH1, numericH2;
129 Matrix actualH1, numericH1;
static int runAllTests(TestResult &result)
Class between(const Class &g) const
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Pose3Upright compose_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
#define EXPECT_LONGS_EQUAL(expected, actual)
Pose3Upright inverse_proxy(const Pose3Upright &x1)
#define EXPECT(condition)
Class compose(const Class &g) const
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Provides additional testing facilities for common data structures.
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Some functions to compute numerical derivatives.
Pose3 inverse() const
inverse transformation with derivatives
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Pose3Upright between_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
TEST(SmartFactorBase, Pinhole)
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)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:54