23 using namespace gtsam;
139 Matrix expectedH, actualH;
166 EXPECT(check_group_invariants(
id,
id));
167 EXPECT(check_group_invariants(
id,T1));
168 EXPECT(check_group_invariants(T2,
id));
169 EXPECT(check_group_invariants(T2,T1));
171 EXPECT(check_manifold_invariants(
id,
id));
172 EXPECT(check_manifold_invariants(
id,T1));
173 EXPECT(check_manifold_invariants(T2,
id));
174 EXPECT(check_manifold_invariants(T2,T1));
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
bool equals(const Rot2 &R, double tol=1e-9) const
Point2 rotate_(const Rot2 &R, const Point2 &p)
Q id(Eigen::AngleAxisd(0, Q_z_axis))
#define DOUBLES_EQUAL(expected, actual, threshold)
EIGEN_DONT_INLINE Scalar zero()
Rot2 R(Rot2::fromAngle(0.1))
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
static const Point3 pt(1.0, 2.0, 3.0)
static Rot2 relativeBearing(const Point2 &d, OptionalJacobian< 1, 2 > H=boost::none)
static Rot2 fromCosSin(double c, double s)
Named constructor from cos(theta),sin(theta) pair, will not normalize!
#define GTSAM_CONCEPT_LIE_INST(T)
static Rot2 atan2(double y, double x)
Point2 unrotate(const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
EIGEN_DEVICE_FUNC const CosReturnType cos() const
Class compose(const Class &g) const
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Matrix2 transpose() const
#define EXPECT(condition)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Point2 unrotate_(const Rot2 &R, const Point2 &p)
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Rot2 relativeBearing_(const Point2 &pt)
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
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
#define CHECK_CHART_DERIVATIVES(t1, t2)
Class between(const Class &g) const
TEST(LPInitSolver, InfiniteLoopSingleVar)
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
EIGEN_DEVICE_FUNC const SinReturnType sin() const
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
#define GTSAM_CONCEPT_TESTABLE_INST(T)
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...