25 using namespace gtsam;
34 OrientedPlane3 plane1(c);
35 OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
37 double distance1 = plane1.
distance();
63 Matrix actualH1, expectedH1, actualH2, expectedH2;
79 size_t numDims = minLimits.size();
80 Vector vector = Vector::Zero(numDims);
83 for (
size_t i = 0;
i < numDims;
i++) {
84 double range = maxLimits(
i) - minLimits(
i);
85 vector(
i) = (((double) rand()) / RAND_MAX) * range + minLimits(
i);
92 size_t numIterations = 10000;
93 Vector4 minPlaneLimit, maxPlaneLimit;
94 minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
95 maxPlaneLimit << 1.0, 1.0, 1.0, 10.0;
100 for (
size_t i = 0;
i < numIterations;
i++) {
106 if (v12.head<3>().norm() >
M_PI)
107 v12.head<3>() = v12.head<3>() / M_PI;
129 Matrix33 actualH1, expectedH1, actualH2, expectedH2;
133 Vector2(actual[0], actual[1])));
136 std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)>
f =
150 std::function<OrientedPlane3(const Vector3&)>
f = [&plane](
const Vector3&
v) {
170 Matrix23 H_actual, H_expected;
173 std::function<Unit3(const OrientedPlane3&)>
f = [](
const OrientedPlane3&
p) {
184 Matrix13 H_actual, H_expected;
187 std::function<double(const OrientedPlane3&)>
f = [](
const OrientedPlane3&
p) {
199 Matrix33 H_retract, H_getters;
208 H_getters << H_normal, H_distance;
214 srand(
time(
nullptr));
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
static int runAllTests(TestResult &result)
static Vector randomVector(const Vector &minLimits, const Vector &maxLimits)
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
TEST(OrientedPlane3, getMethods)
Some functions to compute numerical derivatives.
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
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.
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)
double distance(OptionalJacobian< 1, 3 > H={}) const
Return the perpendicular distance to the origin.
Represents a 3D point on a unit sphere.
#define EXPECT(condition)
Array< int, Dynamic, 1 > v
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
OrientedPlane3 transform_(const OrientedPlane3 &plane, const Pose3 &xr)
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Vector3 localCoordinates(const OrientedPlane3 &s) const
The local coordinates function.
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Double_ range(const Point2_ &p, const Point2_ &q)
Unit3 normal(OptionalJacobian< 2, 3 > H={}) const
Return the normal.
Vector4 planeCoefficients() const
Returns the plane coefficients.
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
OrientedPlane3 retract(const Vector3 &v, OptionalJacobian< 3, 3 > H={}) const
The retract function.
Jet< T, N > sqrt(const Jet< T, N > &f)
OrientedPlane3 transform(const Pose3 &xr, OptionalJacobian< 3, 3 > Hp={}, OptionalJacobian< 3, 6 > Hr={}) const
Vector2 errorVector(const Unit3 &q, OptionalJacobian< 2, 2 > H_p={}, OptionalJacobian< 2, 2 > H_q={}) const
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Vector3 errorVector(const OrientedPlane3 &other, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const