Derivative of multiplying with inverse of matrix
							parent
							
								
									0c9910466e
								
							
						
					
					
						commit
						e6521703e6
					
				|  | @ -12,18 +12,54 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // Generics
 | ||||
| // Generic between, assumes existence of traits<T>::Between
 | ||||
| template<typename T> | ||||
| Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) { | ||||
|   return Expression<T>(traits<T>::Between, t1, t2); | ||||
| } | ||||
| 
 | ||||
| // Generics
 | ||||
| // Generic compose, assumes existence of traits<T>::Compose
 | ||||
| template<typename T> | ||||
| Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) { | ||||
|   return Expression<T>(traits<T>::Compose, t1, t2); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Functor that implements multiplication of a vector b with the inverse of a | ||||
|  * matrix A. The derivatives are inspired by Mike Giles' "An extended collection | ||||
|  * of matrix derivative results for forward and reverse mode algorithmic | ||||
|  * differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf | ||||
|  * | ||||
|  * Usage example: | ||||
|   *  Expression<Vector3> expression = MultiplyWithInverse<3>()(Key(0), Key(1)); | ||||
|  */ | ||||
| template <int N> | ||||
| struct MultiplyWithInverse { | ||||
|   typedef Eigen::Matrix<double, N, 1> VectorN; | ||||
|   typedef Eigen::Matrix<double, N, N> MatrixN; | ||||
| 
 | ||||
|   /// A.inverse() * b, with optional derivatives
 | ||||
|   VectorN operator()(const MatrixN& A, const VectorN& b, | ||||
|                      OptionalJacobian<N, N* N> H1 = boost::none, | ||||
|                      OptionalJacobian<N, N> H2 = boost::none) const { | ||||
|     const MatrixN invA = A.inverse(); | ||||
|     const VectorN c = invA * b; | ||||
|     // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
 | ||||
|     if (H1) | ||||
|       for (size_t j = 0; j < N; j++) | ||||
|         H1->template middleCols<N>(N * j) = -c[j] * invA; | ||||
|     // The derivative in b is easy, as invA*b is just a linear map:
 | ||||
|     if (H2) *H2 = invA; | ||||
|     return c; | ||||
|   } | ||||
| 
 | ||||
|   /// Create expression
 | ||||
|   Expression<VectorN> operator()(const Expression<MatrixN>& A_, | ||||
|                                  const Expression<VectorN>& b_) const { | ||||
|     return Expression<VectorN>(*this, A_, b_); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| typedef Expression<double> double_; | ||||
| typedef Expression<Vector3> Vector3_; | ||||
| 
 | ||||
|  |  | |||
|  | @ -345,7 +345,6 @@ TEST(ExpressionFactor, tree) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| TEST(ExpressionFactor, Compose1) { | ||||
| 
 | ||||
|   // Create expression
 | ||||
|  | @ -600,6 +599,25 @@ TEST(Expression, testMultipleCompositions2) { | |||
|   EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test multiplication with a matrix
 | ||||
| TEST(ExpressionFactor, MultiplyWithInverse) { | ||||
|   // Create expression
 | ||||
|   auto model = noiseModel::Isotropic::Sigma(3, 1); | ||||
|   auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1)); | ||||
| 
 | ||||
|   // Check derivatives
 | ||||
|   Values values; | ||||
|   Matrix3 A = Vector3(1, 2, 3).asDiagonal(); | ||||
|   A(0, 1) = 0.1; | ||||
|   A(0, 2) = 0.1; | ||||
|   const Vector3 b(0.1, 0.2, 0.3); | ||||
|   values.insert<Matrix3>(0, A); | ||||
|   values.insert<Vector3>(1, b); | ||||
|   ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr); | ||||
|   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);  // another way
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue