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

adds test for expression wrappers

release/4.3a0
Brett Downing 2025-02-28 10:50:54 +11:00
parent 5ad287ee62
commit f0295c7c32
1 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
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>
@ -1301,6 +1302,39 @@ TEST(Pose3, interpolateRtJacobians) {
}
}
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));
}
}
/* ************************************************************************* */
TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;