adding jacobians on input points
							parent
							
								
									582afe1dbb
								
							
						
					
					
						commit
						15f8b416cd
					
				|  | @ -101,8 +101,10 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,  //
 | ||||
|                               OptionalJacobian<1, 5> H) const { | ||||
| double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,  | ||||
|                               OptionalJacobian<1, 5> HE, | ||||
|                               OptionalJacobian<1, 3> HvA, | ||||
|                               OptionalJacobian<1, 3> HvB) const { | ||||
|   // compute the epipolar lines
 | ||||
|   Point3 lB = E_ * vB; | ||||
|   Point3 lA = E_.transpose() * vA; | ||||
|  | @ -122,7 +124,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,  // | |||
|   double denominator = sqrt(nA_sq_norm + nB_sq_norm); | ||||
|   double sampson_error = algebraic_error / denominator; | ||||
| 
 | ||||
|   if (H) { | ||||
|   if (HE) { | ||||
|     // See math.lyx
 | ||||
|     // computing the derivatives of the numerator w.r.t. E
 | ||||
|     Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); | ||||
|  | @ -152,9 +154,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,  // | |||
|     Matrix15 numerator_H; | ||||
|     numerator_H << numerator_H_R, numerator_H_D; | ||||
| 
 | ||||
|     *H = numerator_H / denominator - | ||||
|     *HE = numerator_H / denominator - | ||||
|          algebraic_error * denominator_H / (denominator * denominator); | ||||
|   } | ||||
| 
 | ||||
|   if (HvA){ | ||||
|     Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); | ||||
|     Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; | ||||
| 
 | ||||
|     *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); | ||||
|   } | ||||
| 
 | ||||
|   if (HvB){ | ||||
|     Matrix13 numerator_H_vB = vA.transpose() * matrix(); | ||||
|     Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; | ||||
| 
 | ||||
|     *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); | ||||
|   } | ||||
| 
 | ||||
|   return sampson_error; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -161,7 +161,9 @@ class EssentialMatrix { | |||
| 
 | ||||
|   /// epipolar error, sampson
 | ||||
|   GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, | ||||
|       OptionalJacobian<1, 5> H = boost::none) const; | ||||
|       OptionalJacobian<1, 5> HE = boost::none,  | ||||
|       OptionalJacobian<1, 3> HvA = boost::none, | ||||
|       OptionalJacobian<1, 3> HvB = boost::none) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -263,18 +263,14 @@ TEST(EssentialMatrix, errorValue) { | |||
| } | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| double error_(const Rot3& R, const Unit3& t) { | ||||
|   // Use two points to get error
 | ||||
|   Point3 a(1, -2, 1); | ||||
|   Point3 b(3, 1, 1); | ||||
| 
 | ||||
| double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { | ||||
|   EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); | ||||
|   return E.error(a, b); | ||||
| } | ||||
| TEST(EssentialMatrix, errorJacobians) { | ||||
|   // Use two points to get error
 | ||||
|   Point3 a(1, -2, 1); | ||||
|   Point3 b(3, 1, 1); | ||||
|   Point3 vA(1, -2, 1); | ||||
|   Point3 vB(3, 1, 1); | ||||
| 
 | ||||
|   Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); | ||||
|   Point3 c1Tc2(0.4, 0.5, 0.6); | ||||
|  | @ -283,18 +279,27 @@ TEST(EssentialMatrix, errorJacobians) { | |||
|   // Use numerical derivatives to calculate the expected Jacobian
 | ||||
|   Matrix13 HRexpected; | ||||
|   Matrix12 HDexpected; | ||||
|   HRexpected = numericalDerivative21<double, Rot3, Unit3>(error_, E.rotation(), | ||||
|                                                           E.direction()); | ||||
|   HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(), | ||||
|                                                           E.direction()); | ||||
|   Matrix13 HvAexpected; | ||||
|   Matrix13 HvBexpected; | ||||
|   HRexpected = numericalDerivative41<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(), | ||||
|                                                           E.direction(), vA, vB); | ||||
|   HDexpected = numericalDerivative42<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(), | ||||
|                                                           E.direction(), vA, vB); | ||||
|   HvAexpected = numericalDerivative43<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(), | ||||
|                                                           E.direction(), vA, vB); | ||||
|   HvBexpected = numericalDerivative44<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(), | ||||
|                                                           E.direction(), vA, vB); | ||||
|   Matrix15 HEexpected; | ||||
|   HEexpected << HRexpected, HDexpected; | ||||
| 
 | ||||
|   Matrix15 HEactual; | ||||
|   E.error(a, b, HEactual); | ||||
|   Matrix13 HvAactual, HvBactual; | ||||
|   E.error(vA, vB, HEactual, HvAactual, HvBactual); | ||||
| 
 | ||||
|   // Verify the Jacobian is correct
 | ||||
|   EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); | ||||
|   EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); | ||||
|   EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue