fix: return plane coefficents dimension was wrong, should be Vector4, not Vector3
parent
6c34ce1e80
commit
ff14e576ee
|
@ -91,9 +91,9 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 OrientedPlane3::planeCoefficients() const {
|
||||
Vector4 OrientedPlane3::planeCoefficients() const {
|
||||
Vector unit_vec = n_.unitVector();
|
||||
Vector3 a;
|
||||
Vector4 a;
|
||||
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
|
||||
return a;
|
||||
}
|
||||
|
|
|
@ -108,7 +108,7 @@ public:
|
|||
Vector3 localCoordinates(const OrientedPlane3& s) const;
|
||||
|
||||
/// Returns the plane coefficients
|
||||
Vector3 planeCoefficients() const;
|
||||
Vector4 planeCoefficients() const;
|
||||
|
||||
inline Unit3 normal() const {
|
||||
return n_;
|
||||
|
|
|
@ -29,6 +29,18 @@ using boost::none;
|
|||
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
|
||||
|
||||
//*******************************************************************************
|
||||
TEST (OrientedPlane3, get) {
|
||||
Vector4 c;
|
||||
c << -1, 0, 0, 5;
|
||||
OrientedPlane3 plane1(c);
|
||||
OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
|
||||
Vector coefficient1 = plane1.planeCoefficients();
|
||||
EXPECT(assert_equal(coefficient1, c, 1e-8));
|
||||
Vector coefficient2 = plane2.planeCoefficients();
|
||||
EXPECT(assert_equal(coefficient2, c, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST (OrientedPlane3, transform) {
|
||||
// Test transforming a plane to a pose
|
||||
|
|
Loading…
Reference in New Issue