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

added interpolation function from shteren1

release/4.3a0
lcarlone 2021-07-19 22:30:32 -04:00
parent 16d624d4e1
commit a0ca3387fb
2 changed files with 87 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/base/Lie.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Mike Bosse
* @author Duy Nguyen Ta
* @author shteren1
*/
@ -322,8 +323,30 @@ T expm(const Vector& x, int K=7) {
* Linear interpolation between X and Y by coefficient t in [0, 1].
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
T interpolate(const T& X, const T& Y, double t,
OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hx = boost::none,
OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hy = boost::none) {
assert(t >= 0.0 && t <= 1.0);
if (Hx || Hy) {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<T>::dimension> Jacobian;
typename traits<T>::TangentVector log_Xinv_Y;
Jacobian Hx_tmp, Hy_tmp, H1, H2;
T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp);
log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1);
Hx_tmp = H1 * Hx_tmp;
Hy_tmp = H1 * Hy_tmp;
Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, H1);
Hx_tmp = t * H1 * Hx_tmp;
Hy_tmp = t * H1 * Hy_tmp;
Xinv_Y = traits<T>::Compose(X, Xinv_Y, H1, H2);
Hx_tmp = H1 + H2 * Hx_tmp;
Hy_tmp = H2 * Hy_tmp;
if(Hx) *Hx = Hx_tmp;
if(Hy) *Hy = Hy_tmp;
return Xinv_Y;
}
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}

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

@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) {
EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
}
/* ************************************************************************* */
Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
TEST(Pose3, interpolateJacobians) {
{
Pose3 X = Pose3::identity();
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 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));
}
{
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 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));
}
{
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 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));
}
{
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 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));
}
}
/* ************************************************************************* */
TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;