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

refactor tests and add comments

release/4.3a0
yetongumich 2021-05-12 15:36:12 -04:00
parent 32acaecdc9
commit a637737d5e
2 changed files with 81 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -58,22 +58,23 @@ TEST(Expression, Constant) {
/* ************************************************************************* */
// Leaf
TEST(Expression, Leaf) {
Rot3_ R(100);
const Key key = 100;
Rot3_ R(key);
Values values;
values.insert(100, someR);
values.insert(key, someR);
Rot3 actual2 = R.value(values);
EXPECT(assert_equal(someR, actual2));
}
/* ************************************************************************* */
// Many Leaves
// Test the function `createUnknowns` to create many leaves at once.
TEST(Expression, Leaves) {
Values values;
Point3 somePoint(1, 2, 3);
const Point3 somePoint(1, 2, 3);
values.insert(Symbol('p', 10), somePoint);
std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint, points.back().value(values)));
std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
}
/* ************************************************************************* */
@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p;
}
Point3_ p(1);
Point3_ pointExpression(1);
set<Key> expected = list_of(1);
} // namespace unary
// Create a unary expression that takes another expression as a single argument.
TEST(Expression, Unary1) {
using namespace unary;
Expression<Point2> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
using namespace unary;
Double_ e(f2, p);
EXPECT(expected == e.keys());
Expression<Point2> unaryExpression(f1, pointExpression);
EXPECT(expected == unaryExpression.keys());
}
// Check that also works with a scalar return value.
TEST(Expression, Unary2) {
using namespace unary;
Double_ unaryExpression(f2, pointExpression);
EXPECT(expected == unaryExpression.keys());
}
/* ************************************************************************* */
// Unary(Leaf), dynamic
TEST(Expression, Unary3) {
using namespace unary;
// Expression<Vector> e(f3, p);
// TODO(yetongumich): dynamic output arguments do not work yet!
// Expression<Vector> unaryExpression(f3, pointExpression);
// EXPECT(expected == unaryExpression.keys());
}
/* ************************************************************************* */
// Simple test class that implements the `VectorSpace` protocol.
class Class : public Point3 {
public:
enum {dimension = 3};
@ -133,16 +139,20 @@ template<> struct traits<Class> : public internal::VectorSpace<Class> {};
// Nullary Method
TEST(Expression, NullaryMethod) {
// Create expression
Expression<Class> p(67);
Expression<double> norm_(p, &Class::norm);
const Key key(67);
Expression<Class> classExpression(key);
// Make expression from a class method, note how it differs from the function
// expressions by leading with the class expression in the constructor.
Expression<double> norm_(classExpression, &Class::norm);
// Create Values
Values values;
values.insert(67, Class(3, 4, 5));
values.insert(key, Class(3, 4, 5));
// Check dims as map
std::map<Key, int> map;
norm_.dims(map);
norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
LONGS_EQUAL(1, map.size());
// Get value and Jacobians
@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) {
double actual = norm_.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
const double norm = sqrt(3*3 + 4*4 + 5*5);
EXPECT(actual == norm);
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
EXPECT(assert_equal(expected, H[0]));
}
@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p);
}
/* ************************************************************************* */
// Check that creating an expression to double compiles
// Check that creating an expression to double compiles.
TEST(Expression, BinaryToDouble) {
using namespace binary;
Double_ p_cam(doubleF, x, p);
}
/* ************************************************************************* */
// keys
// Check keys of an expression created from class method.
TEST(Expression, BinaryKeys) {
set<Key> expected = list_of(1)(2);
EXPECT(expected == binary::p_cam.keys());
}
/* ************************************************************************* */
// dimensions
// Check dimensions by calling `dims` method.
TEST(Expression, BinaryDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
}
/* ************************************************************************* */
// dimensions
// Check dimensions by calling `traceSize` method.
TEST(Expression, BinaryTraceSize) {
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
size_t expectedTraceSize = sizeof(Binary::Record);
@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) {
}
/* ************************************************************************* */
// Test compose operation with * operator.
TEST(Expression, compose1) {
// Create expression
Rot3_ R1(1), R2(2);
@ -258,7 +270,7 @@ TEST(Expression, compose1) {
}
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
// Test compose with arguments referring to the same rotation.
TEST(Expression, compose2) {
// Create expression
Rot3_ R1(1), R2(1);
@ -270,7 +282,7 @@ TEST(Expression, compose2) {
}
/* ************************************************************************* */
// Test compose with one arguments referring to constant rotation
// Test compose with one arguments referring to constant rotation.
TEST(Expression, compose3) {
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
@ -282,7 +294,7 @@ TEST(Expression, compose3) {
}
/* ************************************************************************* */
// Test with ternary function
// Test with ternary function.
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
@ -306,6 +318,7 @@ TEST(Expression, ternary) {
}
/* ************************************************************************* */
// Test scalar multiplication with * operator.
TEST(Expression, ScalarMultiply) {
const Key key(67);
const Point3_ expr = 23 * Point3_(key);
@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) {
}
/* ************************************************************************* */
// Test sum with + operator.
TEST(Expression, BinarySum) {
const Key key(67);
const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
@ -366,6 +380,7 @@ TEST(Expression, BinarySum) {
}
/* ************************************************************************* */
// Test sum of 3 variables with + operator.
TEST(Expression, TripleSum) {
const Key key(67);
const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@ -387,6 +402,7 @@ TEST(Expression, TripleSum) {
}
/* ************************************************************************* */
// Test sum with += operator.
TEST(Expression, PlusEqual) {
const Key key(67);
const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@ -454,6 +470,7 @@ TEST(Expression, UnaryOfSum) {
/* ************************************************************************* */
TEST(Expression, WeightedSum) {
const Key key1(42), key2(67);
const Point3 point1(1, 0, 0), point2(0, 1, 0);
const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2);
map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
@ -461,11 +478,11 @@ TEST(Expression, WeightedSum) {
EXPECT(actual_dims == expected_dims);
Values values;
values.insert<Point3>(key1, Point3(1, 0, 0));
values.insert<Point3>(key2, Point3(0, 1, 0));
values.insert<Point3>(key1, point1);
values.insert<Point3>(key2, point2);
// Check value
const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
const Point3 expected = 17 * point1 + 23 * point2;
EXPECT(assert_equal(expected, weighted_sum_.value(values)));
// Check value + Jacobians

59
tests/testExpressionFactor.cpp
View File

@ -59,40 +59,42 @@ Point2_ p(2);
TEST(ExpressionFactor, Leaf) {
using namespace leaf;
// Create old-style factor to create expected value and derivatives
// Create old-style factor to create expected value and derivatives.
PriorFactor<Point2> old(2, Point2(0, 0), model);
// Concise version
// Create the equivalent factor with expression.
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives.
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
}
/* ************************************************************************* */
// non-zero noise model
// Test leaf expression with noise model of differnt variance.
TEST(ExpressionFactor, Model) {
using namespace leaf;
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
// Create old-style factor to create expected value and derivatives
// Create old-style factor to create expected value and derivatives.
PriorFactor<Point2> old(2, Point2(0, 0), model);
// Concise version
// Create the equivalent factor with expression.
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives
// Check values and derivatives.
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
}
/* ************************************************************************* */
// Constrained noise model
// Test leaf expression with constrained noise model.
TEST(ExpressionFactor, Constrained) {
using namespace leaf;
@ -106,7 +108,7 @@ TEST(ExpressionFactor, Constrained) {
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
}
/* ************************************************************************* */
@ -130,7 +132,7 @@ TEST(ExpressionFactor, Unary) {
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf, 1e-9));
EXPECT(assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
@ -143,11 +145,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
if (H) *H << I_3x3, I_3x3, I_3x3;
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
if (H) *H = Matrix9::Identity();
return v;
}
TEST(ExpressionFactor, Wide) {
// Create some values
Values values;
@ -208,6 +212,7 @@ TEST(ExpressionFactor, Binary) {
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))
TEST(ExpressionFactor, Shallow) {
@ -264,7 +269,7 @@ TEST(ExpressionFactor, Shallow) {
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
EXPECT(assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */
@ -297,7 +302,7 @@ TEST(ExpressionFactor, tree) {
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
EXPECT(assert_equal(*expected, *gf, 1e-9));
// Concise version
ExpressionFactor<Point2> f2(model, measured,
@ -305,14 +310,14 @@ TEST(ExpressionFactor, tree) {
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
EXPECT(assert_equal(*expected, *gf2, 1e-9));
// Try ternary version
ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f3.dim());
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
EXPECT( assert_equal(*expected, *gf3, 1e-9));
EXPECT(assert_equal(*expected, *gf3, 1e-9));
}
/* ************************************************************************* */
@ -333,15 +338,15 @@ TEST(ExpressionFactor, Compose1) {
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[1],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -363,14 +368,14 @@ TEST(ExpressionFactor, compose2) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
EXPECT(assert_equal(2*I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -392,14 +397,14 @@ TEST(ExpressionFactor, compose3) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
@ -435,16 +440,16 @@ TEST(ExpressionFactor, composeTernary) {
std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
EXPECT( assert_equal(I_3x3, H[2],1e-9));
EXPECT(assert_equal(I_3x3, H[0],1e-9));
EXPECT(assert_equal(I_3x3, H[1],1e-9));
EXPECT(assert_equal(I_3x3, H[2],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT(assert_equal(expected, *jf,1e-9));
}
TEST(ExpressionFactor, tree_finite_differences) {
@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
class TestNaryFactor
: public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
gtsam::Rot3, gtsam::Point3,
gtsam::Rot3,gtsam::Point3> {
gtsam::Rot3, gtsam::Point3> {
private:
using This = TestNaryFactor;
using Base =