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