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

Merge pull request #910 from borglab/feature/lie-interpolate-tests

release/4.3a0
Varun Agrawal 2021-10-26 00:32:11 -04:00 committed by GitHub
parent 59abf8aebc 27143fc724
commit 4fc9e830ec
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
tests/testLie.cpp
View File

@ -129,6 +129,46 @@ TEST( testProduct, Logmap ) {
EXPECT(assert_equal(numericH, actH, tol));
}
/* ************************************************************************* */
Product interpolate_proxy(const Product& x, const Product& y, double t) {
return interpolate<Product>(x, y, t);
}
TEST(Lie, Interpolate) {
Product x(Point2(1, 2), Pose2(3, 4, 5));
Product y(Point2(6, 7), Pose2(8, 9, 0));
double t;
Matrix actH1, numericH1, actH2, numericH2;
t = 0.0;
interpolate<Product>(x, y, t, actH1, actH2);
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));
t = 0.5;
interpolate<Product>(x, y, t, actH1, actH2);
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));
t = 1.0;
interpolate<Product>(x, y, t, actH1, actH2);
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));
}
//******************************************************************************
int main() {
TestResult tr;