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

Merge pull request #2035 from BrettRD/develop 

Adds Jacobians to Lie::interpolate and others
release/4.3a0
Frank Dellaert 2025-02-27 22:49:06 -05:00 committed by GitHub
parent a83c50dc6c f0295c7c32
commit b937790351
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
6 changed files with 177 additions and 14 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>
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = {},
typename MakeOptionalJacobian<T, T>::type Hy = {}) {
if (Hx || Hy) {
typename MakeOptionalJacobian<T, T>::type 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;
const T between =
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 (Hy) *Hy = t * exp_H * log_H;
if (Ht) *Ht = delta;
return result;
}
return traits<T>::Compose(

22
gtsam/geometry/Pose3.cpp
View File

@ -171,9 +171,29 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
}
/* ************************************************************************* */
Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
Pose3 Pose3::interpolateRt(const Pose3& T, double t,
OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> Harg,
OptionalJacobian<6, 1> Ht) const {
if(Hself || Harg || Ht){
typename MakeJacobian<Rot3, Rot3>::type HselfRot, HargRot;
typename MakeJacobian<Rot3, double>::type HtRot;
typename MakeJacobian<Point3, Point3>::type HselfPoint, HargPoint;
typename MakeJacobian<Point3, double>::type HtPoint;
Rot3 Rint = interpolate<Rot3>(R_, T.R_, t, HselfRot, HargRot, HtRot);
Point3 Pint = interpolate<Point3>(t_, T.t_, t, HselfPoint, HargPoint, HtPoint);
Pose3 result = Pose3(Rint, Pint);
if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint;
if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, Rint.transpose() * T.R_.matrix() * HargPoint;
if(Ht) *Ht << HtRot, Rint.transpose() * HtPoint;
return result;
}
return Pose3(interpolate<Rot3>(R_, T.R_, t),
interpolate<Point3>(t_, T.t_, t));
}
/* ************************************************************************* */

5
gtsam/geometry/Pose3.h
View File

@ -132,7 +132,10 @@ public:
* @param T End point of interpolation.
* @param t A value in [0, 1].
*/
Pose3 interpolateRt(const Pose3& T, double t) const;
Pose3 interpolateRt(const Pose3& T, double t,
OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> Harg = {},
OptionalJacobian<6, 1> Ht = {}) const;
/// @}
/// @name Lie Group

131
gtsam/geometry/tests/testPose3.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/slam/expressions.h>
#include <CppUnitLite/TestHarness.h>
@ -1167,56 +1168,170 @@ TEST(Pose3, interpolateJacobians) {
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
interpolate(X, Y, t, actualJacobianX, actualJacobianY);
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
}
/* ************************************************************************* */
Pose3 testing_interpolate_rt(const Pose3& t1, const Pose3& t2, double gamma) { return t1.interpolateRt(t2, gamma); }
TEST(Pose3, interpolateRtJacobians) {
{
Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
double t = 0.5;
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
double t = 0.3;
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
double t = 0.5;
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
{
Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
double t = 0.3;
Matrix actualJacobianX, actualJacobianY, actualJacobianT;
X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
}
}
TEST(Pose3, expressionWrappers) {
Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
double t = 0.3;
Values vals;
vals.insert(0,X);
vals.insert(1,Y);
vals.insert(2,t);
{ // interpolate (templated wrapper applies to all classes)
Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
std::vector<Matrix> Hlist = {{},{},{}};
Pose3 expected = interpolate(X, Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
Pose3 actual = interpolate(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
EXPECT(assert_equal(expected,actual,1e-6));
EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
}
{ // interpolateRt (Pose3 specialisation)
Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
std::vector<Matrix> Hlist = {{},{},{}};
Pose3 expected = X.interpolateRt(Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
Pose3 actual = interpolateRt(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
EXPECT(assert_equal(expected,actual,1e-6));
EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
}
}

14
gtsam/slam/expressions.h
View File

@ -58,6 +58,10 @@ inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) {
return Pose3_(p, &Pose3::transformPoseTo, q);
}
inline Pose3_ interpolateRt(const Pose3_& p, const Pose3_& q, const Double_& t) {
return Pose3_(&Pose3::interpolateRt, p, q, t);
}
inline Point3_ normalize(const Point3_& a){
Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
return Point3_(f, a);
@ -195,4 +199,14 @@ gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
gtsam::traits<T>::Logmap, between(x1, x2));
}
template <typename T>
inline Expression<T> interpolate(const Expression<T>& p, const Expression<T>& q,
const Expression<double>& t){
T (*f)(const T&, const T&, double,
typename MakeOptionalJacobian<T, T>::type,
typename MakeOptionalJacobian<T, T>::type,
typename MakeOptionalJacobian<T, double>::type) = &interpolate;
return Expression<T>(f, p, q, t);
}
} // \namespace gtsam

13
tests/testLie.cpp
View File

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