12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

adds jacobian for t in Lie::Interpolate

release/4.3a0
Brett Downing 2025-02-27 13:40:13 +11:00
parent 08147c7011
commit fffb7ae69d
2 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/base/Lie.h
View File

@ -326,8 +326,9 @@ T expm(const Vector& x, int K=7) {
template <typename T> template <typename T>
T interpolate(const T& X, const T& Y, double t, T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = {}, typename MakeOptionalJacobian<T, T>::type Hx = {},
typename MakeOptionalJacobian<T, T>::type Hy = {}) { typename MakeOptionalJacobian<T, T>::type Hy = {},
if (Hx || Hy) { typename MakeOptionalJacobian<T, double>::type Ht = {}) {
if (Hx || Hy || Ht) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x; typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between = const T between =
traits<T>::Between(X, Y, between_H_x); // between_H_y = identity traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
@ -338,6 +339,7 @@ T interpolate(const T& X, const T& Y, double t,
if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
if (Hy) *Hy = t * exp_H * log_H; if (Hy) *Hy = t * exp_H * log_H;
if (Ht) *Ht = delta;
return result; return result;
} }
return traits<T>::Compose( return traits<T>::Compose(

13
tests/testLie.cpp
View File

@ -139,16 +139,19 @@ TEST(Lie, Interpolate) {
Product y(Point2(6, 7), Pose2(8, 9, 0)); Product y(Point2(6, 7), Pose2(8, 9, 0));
double t; double t;
Matrix actH1, numericH1, actH2, numericH2; Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3;
t = 0.0; t = 0.0;
interpolate<Product>(x, y, t, actH1, actH2); interpolate<Product>(x, y, t, actH1, actH2, actH3);
numericH1 = numericalDerivative31<Product, Product, Product, double>( numericH1 = numericalDerivative31<Product, Product, Product, double>(
interpolate_proxy, x, y, t); interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH1, actH1, tol)); EXPECT(assert_equal(numericH1, actH1, tol));
numericH2 = numericalDerivative32<Product, Product, Product, double>( numericH2 = numericalDerivative32<Product, Product, Product, double>(
interpolate_proxy, x, y, t); interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH2, actH2, tol)); EXPECT(assert_equal(numericH2, actH2, tol));
numericH3 = numericalDerivative33<Product, Product, Product, double>(
interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH3, actH3, tol));
t = 0.5; t = 0.5;
interpolate<Product>(x, y, t, actH1, actH2); interpolate<Product>(x, y, t, actH1, actH2);
@ -158,6 +161,9 @@ TEST(Lie, Interpolate) {
numericH2 = numericalDerivative32<Product, Product, Product, double>( numericH2 = numericalDerivative32<Product, Product, Product, double>(
interpolate_proxy, x, y, t); interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH2, actH2, tol)); EXPECT(assert_equal(numericH2, actH2, tol));
numericH3 = numericalDerivative33<Product, Product, Product, double>(
interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH3, actH3, tol));
t = 1.0; t = 1.0;
interpolate<Product>(x, y, t, actH1, actH2); interpolate<Product>(x, y, t, actH1, actH2);
@ -167,6 +173,9 @@ TEST(Lie, Interpolate) {
numericH2 = numericalDerivative32<Product, Product, Product, double>( numericH2 = numericalDerivative32<Product, Product, Product, double>(
interpolate_proxy, x, y, t); interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH2, actH2, tol)); EXPECT(assert_equal(numericH2, actH2, tol));
numericH3 = numericalDerivative33<Product, Product, Product, double>(
interpolate_proxy, x, y, t);
EXPECT(assert_equal(numericH3, actH3, tol));
} }
//****************************************************************************** //******************************************************************************