23 #include <boost/assign/std/vector.hpp> 26 using namespace gtsam;
37 OrientedPlane3 plane1(c);
38 OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
40 double distance1 = plane1.
distance();
66 Matrix actualH1, expectedH1, actualH2, expectedH2;
82 size_t numDims = minLimits.size();
83 Vector vector = Vector::Zero(numDims);
86 for (
size_t i = 0;
i < numDims;
i++) {
87 double range = maxLimits(
i) - minLimits(
i);
88 vector(
i) = (((double) rand()) / RAND_MAX) * range + minLimits(
i);
95 size_t numIterations = 10000;
96 Vector4 minPlaneLimit, maxPlaneLimit;
97 minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
98 maxPlaneLimit << 1.0, 1.0, 1.0, 10.0;
100 Vector3 minXiLimit, maxXiLimit;
103 for (
size_t i = 0;
i < numIterations;
i++) {
109 if (v12.head<3>().norm() >
M_PI)
110 v12.head<3>() = v12.head<3>() / M_PI;
132 Matrix33 actualH1, expectedH1, actualH2, expectedH2;
136 Vector2(actual[0], actual[1])));
139 boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)>
f =
140 boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
151 boost::function<OrientedPlane3(const Vector3&)>
f =
152 boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
169 srand(
time(
nullptr));
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
static int runAllTests(TestResult &result)
static Vector randomVector(const Vector &minLimits, const Vector &maxLimits)
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
TEST(OrientedPlane3, getMethods)
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Some functions to compute numerical derivatives.
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)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 > Hr=boost::none)
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Represents a 3D point on a unit sphere.
OrientedPlane3 retract(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none) const
The retract function.
OrientedPlane3 transform(const Pose3 &xr, OptionalJacobian< 3, 3 > Hp=boost::none, OptionalJacobian< 3, 6 > Hr=boost::none) 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)
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
GTSAM_EXPORT Vector2 errorVector(const Unit3 &q, OptionalJacobian< 2, 2 > H_p=boost::none, OptionalJacobian< 2, 2 > H_q=boost::none) const
#define EXPECT(condition)
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)
double distance() const
Return the perpendicular distance to the origin.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Vector3 errorVector(const OrientedPlane3 &other, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Vector4 planeCoefficients() const
Returns the plane coefficients.
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.
Unit3 normal() const
Return the normal.
Vector3 localCoordinates(const OrientedPlane3 &s) const
The local coordinates function.
#define GTSAM_CONCEPT_TESTABLE_INST(T)