Fix warnings and use new allocAligned scheme
Frank Dellaert
parent
73c20c2ae1
commit
ae1526dd8a
|
|
@ -47,7 +47,7 @@ TEST(Expression, Constant) {
|
|||
Rot3_ R(someR);
|
||||
Values values;
|
||||
Rot3 actual = R.value(values);
|
||||
EXPECT(assert_equal(someR, actual));
|
||||
EXPECT(assert_equal(someR, actual))
|
||||
EXPECT_LONGS_EQUAL(0, R.traceSize())
|
||||
}
|
||||
|
||||
|
|
@ -60,7 +60,7 @@ TEST(Expression, Leaf) {
|
|||
values.insert(key, someR);
|
||||
|
||||
Rot3 actual2 = R.value(values);
|
||||
EXPECT(assert_equal(someR, actual2));
|
||||
EXPECT(assert_equal(someR, actual2))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -70,7 +70,7 @@ TEST(Expression, Leaves) {
|
|||
const Point3 somePoint(1, 2, 3);
|
||||
values.insert(Symbol('p', 10), somePoint);
|
||||
std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
|
||||
EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
|
||||
EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -93,14 +93,14 @@ const set<Key> expected{1};
|
|||
TEST(Expression, Unary1) {
|
||||
using namespace unary;
|
||||
Expression<Point2> unaryExpression(f1, pointExpression);
|
||||
EXPECT(expected == unaryExpression.keys());
|
||||
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());
|
||||
EXPECT(expected == unaryExpression.keys())
|
||||
}
|
||||
|
||||
// Unary(Leaf), dynamic
|
||||
|
|
@ -108,7 +108,7 @@ TEST(Expression, Unary3) {
|
|||
using namespace unary;
|
||||
// TODO(yetongumich): dynamic output arguments do not work yet!
|
||||
// Expression<Vector> unaryExpression(f3, pointExpression);
|
||||
// EXPECT(expected == unaryExpression.keys());
|
||||
// EXPECT(expected == unaryExpression.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -149,7 +149,7 @@ TEST(Expression, NullaryMethod) {
|
|||
// Check dims as map
|
||||
std::map<Key, int> map;
|
||||
norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
|
||||
LONGS_EQUAL(1, map.size());
|
||||
LONGS_EQUAL(1, map.size())
|
||||
|
||||
// Get value and Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
|
|
@ -157,10 +157,10 @@ TEST(Expression, NullaryMethod) {
|
|||
|
||||
// Check all
|
||||
const double norm = sqrt(3*3 + 4*4 + 5*5);
|
||||
EXPECT(actual == norm);
|
||||
EXPECT(actual == norm)
|
||||
Matrix expected(1, 3);
|
||||
expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
|
||||
EXPECT(assert_equal(expected, H[0]));
|
||||
EXPECT(assert_equal(expected, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -187,7 +187,7 @@ TEST(Expression, BinaryToDouble) {
|
|||
// Check keys of an expression created from class method.
|
||||
TEST(Expression, BinaryKeys) {
|
||||
const set<Key> expected{1, 2};
|
||||
EXPECT(expected == binary::p_cam.keys());
|
||||
EXPECT(expected == binary::p_cam.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -195,7 +195,7 @@ TEST(Expression, BinaryKeys) {
|
|||
TEST(Expression, BinaryDimensions) {
|
||||
map<Key, int> actual, expected{{1, 6}, {2, 3}};
|
||||
binary::p_cam.dims(actual);
|
||||
EXPECT(actual == expected);
|
||||
EXPECT(actual == expected)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -204,7 +204,7 @@ TEST(Expression, BinaryTraceSize) {
|
|||
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
|
||||
size_t expectedTraceSize = sizeof(Binary::Record);
|
||||
internal::upAlign(expectedTraceSize);
|
||||
EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
|
||||
EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -224,7 +224,7 @@ Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
|
|||
// keys
|
||||
TEST(Expression, TreeKeys) {
|
||||
const set<Key> expected{1, 2, 3};
|
||||
EXPECT(expected == tree::uv_hat.keys());
|
||||
EXPECT(expected == tree::uv_hat.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -232,25 +232,25 @@ TEST(Expression, TreeKeys) {
|
|||
TEST(Expression, TreeDimensions) {
|
||||
map<Key, int> actual, expected{{1, 6}, {2, 3}, {3, 5}};
|
||||
tree::uv_hat.dims(actual);
|
||||
EXPECT(actual == expected);
|
||||
EXPECT(actual == expected)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TraceSize
|
||||
TEST(Expression, TreeTraceSize) {
|
||||
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
|
||||
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), tree::p_cam.traceSize());
|
||||
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), tree::p_cam.traceSize())
|
||||
|
||||
typedef internal::UnaryExpression<Point2, Point3> Unary;
|
||||
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(),
|
||||
tree::projection.traceSize());
|
||||
tree::projection.traceSize())
|
||||
|
||||
EXPECT_LONGS_EQUAL(0, tree::K.traceSize());
|
||||
EXPECT_LONGS_EQUAL(0, tree::K.traceSize())
|
||||
|
||||
typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
|
||||
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() +
|
||||
tree::projection.traceSize(),
|
||||
tree::uv_hat.traceSize());
|
||||
tree::uv_hat.traceSize())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -262,7 +262,7 @@ TEST(Expression, compose1) {
|
|||
|
||||
// Check keys
|
||||
const set<Key> expected{1, 2};
|
||||
EXPECT(expected == R3.keys());
|
||||
EXPECT(expected == R3.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -274,7 +274,7 @@ TEST(Expression, compose2) {
|
|||
|
||||
// Check keys
|
||||
const set<Key> expected{1};
|
||||
EXPECT(expected == R3.keys());
|
||||
EXPECT(expected == R3.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -286,7 +286,7 @@ TEST(Expression, compose3) {
|
|||
|
||||
// Check keys
|
||||
const set<Key> expected{3};
|
||||
EXPECT(expected == R3.keys());
|
||||
EXPECT(expected == R3.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -299,7 +299,7 @@ TEST(Expression, compose4) {
|
|||
|
||||
// Check keys
|
||||
const set<Key> expected{1};
|
||||
EXPECT(expected == R3.keys());
|
||||
EXPECT(expected == R3.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -323,7 +323,7 @@ TEST(Expression, ternary) {
|
|||
|
||||
// Check keys
|
||||
const set<Key> expected {1, 2, 3};
|
||||
EXPECT(expected == ABC.keys());
|
||||
EXPECT(expected == ABC.keys())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -333,28 +333,28 @@ TEST(Expression, ScalarMultiply) {
|
|||
const Point3_ expr = 23 * Point3_(key);
|
||||
|
||||
const set<Key> expected_keys{key};
|
||||
EXPECT(expected_keys == expr.keys());
|
||||
EXPECT(expected_keys == expr.keys())
|
||||
|
||||
map<Key, int> actual_dims, expected_dims {{key, 3}};
|
||||
expr.dims(actual_dims);
|
||||
EXPECT(actual_dims == expected_dims);
|
||||
EXPECT(actual_dims == expected_dims)
|
||||
|
||||
// Check dims as map
|
||||
std::map<Key, int> map;
|
||||
expr.dims(map);
|
||||
LONGS_EQUAL(1, map.size());
|
||||
LONGS_EQUAL(1, map.size())
|
||||
|
||||
Values values;
|
||||
values.insert<Point3>(key, Point3(1, 0, 2));
|
||||
|
||||
// Check value
|
||||
const Point3 expected(23, 0, 46);
|
||||
EXPECT(assert_equal(expected, expr.value(values)));
|
||||
EXPECT(assert_equal(expected, expr.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT(assert_equal(expected, expr.value(values, H)));
|
||||
EXPECT(assert_equal(23 * I_3x3, H[0]));
|
||||
EXPECT(assert_equal(expected, expr.value(values, H)))
|
||||
EXPECT(assert_equal(23 * I_3x3, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -364,28 +364,28 @@ TEST(Expression, BinarySum) {
|
|||
const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
|
||||
|
||||
const set<Key> expected_keys{key};
|
||||
EXPECT(expected_keys == sum_.keys());
|
||||
EXPECT(expected_keys == sum_.keys())
|
||||
|
||||
map<Key, int> actual_dims, expected_dims {{key, 3}};
|
||||
sum_.dims(actual_dims);
|
||||
EXPECT(actual_dims == expected_dims);
|
||||
EXPECT(actual_dims == expected_dims)
|
||||
|
||||
// Check dims as map
|
||||
std::map<Key, int> map;
|
||||
sum_.dims(map);
|
||||
LONGS_EQUAL(1, map.size());
|
||||
LONGS_EQUAL(1, map.size())
|
||||
|
||||
Values values;
|
||||
values.insert<Point3>(key, Point3(2, 2, 2));
|
||||
|
||||
// Check value
|
||||
const Point3 expected(3, 3, 3);
|
||||
EXPECT(assert_equal(expected, sum_.value(values)));
|
||||
EXPECT(assert_equal(expected, sum_.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)));
|
||||
EXPECT(assert_equal(I_3x3, H[0]));
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)))
|
||||
EXPECT(assert_equal(I_3x3, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -395,19 +395,19 @@ TEST(Expression, TripleSum) {
|
|||
const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
|
||||
const Expression<Point3> sum_ = p1_ + p2_ + p1_;
|
||||
|
||||
LONGS_EQUAL(1, sum_.keys().size());
|
||||
LONGS_EQUAL(1, sum_.keys().size())
|
||||
|
||||
Values values;
|
||||
values.insert<Point3>(key, Point3(2, 2, 2));
|
||||
|
||||
// Check value
|
||||
const Point3 expected(4, 4, 4);
|
||||
EXPECT(assert_equal(expected, sum_.value(values)));
|
||||
EXPECT(assert_equal(expected, sum_.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)));
|
||||
EXPECT(assert_equal(I_3x3, H[0]));
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)))
|
||||
EXPECT(assert_equal(I_3x3, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -419,19 +419,19 @@ TEST(Expression, PlusEqual) {
|
|||
sum_ += p2_;
|
||||
sum_ += p1_;
|
||||
|
||||
LONGS_EQUAL(1, sum_.keys().size());
|
||||
LONGS_EQUAL(1, sum_.keys().size())
|
||||
|
||||
Values values;
|
||||
values.insert<Point3>(key, Point3(2, 2, 2));
|
||||
|
||||
// Check value
|
||||
const Point3 expected(4, 4, 4);
|
||||
EXPECT(assert_equal(expected, sum_.value(values)));
|
||||
EXPECT(assert_equal(expected, sum_.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)));
|
||||
EXPECT(assert_equal(I_3x3, H[0]));
|
||||
EXPECT(assert_equal(expected, sum_.value(values, H)))
|
||||
EXPECT(assert_equal(I_3x3, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -444,12 +444,12 @@ TEST(Expression, SumOfUnaries) {
|
|||
values.insert<Point3>(key, Point3(6, 0, 0));
|
||||
|
||||
// Check value
|
||||
EXPECT_DOUBLES_EQUAL(12, sum_.value(values), 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(12, sum_.value(values), 1e-9)
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT_DOUBLES_EQUAL(12, sum_.value(values, H), 1e-9);
|
||||
EXPECT(assert_equal(Vector3(2, 0, 0).transpose(), H[0]));
|
||||
EXPECT_DOUBLES_EQUAL(12, sum_.value(values, H), 1e-9)
|
||||
EXPECT(assert_equal(Vector3(2, 0, 0).transpose(), H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -460,20 +460,20 @@ TEST(Expression, UnaryOfSum) {
|
|||
|
||||
map<Key, int> actual_dims, expected_dims = {{key1, 3}, {key2, 3}};
|
||||
norm_.dims(actual_dims);
|
||||
EXPECT(actual_dims == expected_dims);
|
||||
EXPECT(actual_dims == expected_dims)
|
||||
|
||||
Values values;
|
||||
values.insert<Point3>(key1, Point3(1, 0, 0));
|
||||
values.insert<Point3>(key2, Point3(0, 1, 0));
|
||||
|
||||
// Check value
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values), 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values), 1e-9)
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(2);
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values, H), 1e-9);
|
||||
EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[0]));
|
||||
EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[1]));
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values, H), 1e-9)
|
||||
EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[0]))
|
||||
EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[1]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -483,7 +483,7 @@ TEST(Expression, WeightedSum) {
|
|||
|
||||
map<Key, int> actual_dims, expected_dims {{key1, 3}, {key2, 3}};
|
||||
weighted_sum_.dims(actual_dims);
|
||||
EXPECT(actual_dims == expected_dims);
|
||||
EXPECT(actual_dims == expected_dims)
|
||||
|
||||
Values values;
|
||||
const Point3 point1(1, 0, 0), point2(0, 1, 0);
|
||||
|
|
@ -492,13 +492,13 @@ TEST(Expression, WeightedSum) {
|
|||
|
||||
// Check value
|
||||
const Point3 expected = 17 * point1 + 23 * point2;
|
||||
EXPECT(assert_equal(expected, weighted_sum_.value(values)));
|
||||
EXPECT(assert_equal(expected, weighted_sum_.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(2);
|
||||
EXPECT(assert_equal(expected, weighted_sum_.value(values, H)));
|
||||
EXPECT(assert_equal(17 * I_3x3, H[0]));
|
||||
EXPECT(assert_equal(23 * I_3x3, H[1]));
|
||||
EXPECT(assert_equal(expected, weighted_sum_.value(values, H)))
|
||||
EXPECT(assert_equal(17 * I_3x3, H[0]))
|
||||
EXPECT(assert_equal(23 * I_3x3, H[1]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -509,13 +509,13 @@ TEST(Expression, Subtract) {
|
|||
values.insert(1, q);
|
||||
const Vector3_ expression = Vector3_(0) - Vector3_(1);
|
||||
set<Key> expected_keys = {0, 1};
|
||||
EXPECT(expression.keys() == expected_keys);
|
||||
EXPECT(expression.keys() == expected_keys)
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(2);
|
||||
EXPECT(assert_equal<Vector3>(p - q, expression.value(values, H)));
|
||||
EXPECT(assert_equal(I_3x3, H[0]));
|
||||
EXPECT(assert_equal(-I_3x3, H[1]));
|
||||
EXPECT(assert_equal<Vector3>(p - q, expression.value(values, H)))
|
||||
EXPECT(assert_equal(I_3x3, H[0]))
|
||||
EXPECT(assert_equal(-I_3x3, H[1]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -530,12 +530,12 @@ TEST(Expression, LinearExpression) {
|
|||
|
||||
// Check value
|
||||
const Vector3 expected(1, 0, 2);
|
||||
EXPECT(assert_equal(expected, linear_.value(values)));
|
||||
EXPECT(assert_equal(expected, linear_.value(values)))
|
||||
|
||||
// Check value + Jacobians
|
||||
std::vector<Matrix> H(1);
|
||||
EXPECT(assert_equal(expected, linear_.value(values, H)));
|
||||
EXPECT(assert_equal(I_3x3, H[0]));
|
||||
EXPECT(assert_equal(expected, linear_.value(values, H)))
|
||||
EXPECT(assert_equal(I_3x3, H[0]))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -63,10 +63,10 @@ TEST(ExpressionFactor, Leaf) {
|
|||
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());
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f.dim())
|
||||
std::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))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -83,11 +83,11 @@ TEST(ExpressionFactor, Model) {
|
|||
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());
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f.dim())
|
||||
std::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
|
||||
EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9))
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5) // another way
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -102,10 +102,10 @@ TEST(ExpressionFactor, Constrained) {
|
|||
|
||||
// Concise version
|
||||
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f.dim())
|
||||
std::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))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -125,11 +125,11 @@ TEST(ExpressionFactor, Unary) {
|
|||
|
||||
// Concise version
|
||||
ExpressionFactor<Point2> f(model, measured, project(p));
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
EXPECT_LONGS_EQUAL(2, f.dim())
|
||||
std::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
std::shared_ptr<JacobianFactor> jf = //
|
||||
std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT(assert_equal(expected, *jf, 1e-9));
|
||||
EXPECT(assert_equal(expected, *jf, 1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -160,11 +160,11 @@ TEST(ExpressionFactor, Wide) {
|
|||
SharedNoiseModel model = noiseModel::Unit::Create(9);
|
||||
|
||||
ExpressionFactor<Vector9> f1(model, measured, expression);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9)
|
||||
|
||||
Expression<Vector9> expression2(id9,expression);
|
||||
ExpressionFactor<Vector9> f2(model, measured, expression2);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -188,13 +188,10 @@ TEST(ExpressionFactor, Binary) {
|
|||
values.insert(2, Point2(0, 0));
|
||||
|
||||
// Check size
|
||||
size_t size = binary.traceSize();
|
||||
// Use Variable Length Array, allocated on stack by gcc
|
||||
// Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
|
||||
internal::ExecutionTraceStorage traceStorage[size];
|
||||
internal::ExecutionTrace<Point2> trace;
|
||||
Point2 value = binary.traceExecution(values, trace, traceStorage);
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
|
||||
auto traceStorage = allocAligned(binary.traceSize());
|
||||
Point2 value = binary.traceExecution(values, trace, traceStorage.get());
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9))
|
||||
// trace.print();
|
||||
|
||||
// Expected Jacobians
|
||||
|
|
@ -205,9 +202,9 @@ TEST(ExpressionFactor, Binary) {
|
|||
|
||||
// Check matrices
|
||||
std::optional<Binary::Record*> r = trace.record<Binary::Record>();
|
||||
CHECK(r);
|
||||
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
|
||||
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
|
||||
CHECK(r)
|
||||
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9))
|
||||
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -234,20 +231,19 @@ TEST(ExpressionFactor, Shallow) {
|
|||
|
||||
// Get and check keys and dims
|
||||
const auto [keys, dims] = expression.keysAndDims();
|
||||
LONGS_EQUAL(2,keys.size());
|
||||
LONGS_EQUAL(2,dims.size());
|
||||
LONGS_EQUAL(1,keys[0]);
|
||||
LONGS_EQUAL(2,keys[1]);
|
||||
LONGS_EQUAL(6,dims[0]);
|
||||
LONGS_EQUAL(3,dims[1]);
|
||||
LONGS_EQUAL(2,keys.size())
|
||||
LONGS_EQUAL(2,dims.size())
|
||||
LONGS_EQUAL(1,keys[0])
|
||||
LONGS_EQUAL(2,keys[1])
|
||||
LONGS_EQUAL(6,dims[0])
|
||||
LONGS_EQUAL(3,dims[1])
|
||||
|
||||
// traceExecution of shallow tree
|
||||
typedef internal::UnaryExpression<Point2, Point3> Unary;
|
||||
size_t size = expression.traceSize();
|
||||
internal::ExecutionTraceStorage traceStorage[size];
|
||||
internal::ExecutionTrace<Point2> trace;
|
||||
Point2 value = expression.traceExecution(values, trace, traceStorage);
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
|
||||
auto traceStorage = allocAligned(expression.traceSize());
|
||||
Point2 value = expression.traceExecution(values, trace, traceStorage.get());
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9))
|
||||
// trace.print();
|
||||
|
||||
// Expected Jacobians
|
||||
|
|
@ -256,15 +252,15 @@ TEST(ExpressionFactor, Shallow) {
|
|||
|
||||
// Check matrices
|
||||
std::optional<Unary::Record*> r = trace.record<Unary::Record>();
|
||||
CHECK(r);
|
||||
EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9));
|
||||
CHECK(r)
|
||||
EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9))
|
||||
|
||||
// Linearization
|
||||
ExpressionFactor<Point2> f2(model, measured, expression);
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim())
|
||||
std::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *gf2, 1e-9));
|
||||
EXPECT(assert_equal(*expected, *gf2, 1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -294,25 +290,25 @@ TEST(ExpressionFactor, tree) {
|
|||
|
||||
// Create factor and check value, dimension, linearization
|
||||
ExpressionFactor<Point2> f(model, measured, uv_hat);
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f.dim())
|
||||
std::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,
|
||||
uncalibrate(K, project(transformTo(x, p))));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim())
|
||||
std::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());
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9)
|
||||
EXPECT_LONGS_EQUAL(2, f3.dim())
|
||||
std::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
|
||||
EXPECT(assert_equal(*expected, *gf3, 1e-9));
|
||||
EXPECT(assert_equal(*expected, *gf3, 1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -333,15 +329,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);
|
||||
std::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
std::shared_ptr<JacobianFactor> jf = //
|
||||
std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT(assert_equal(expected, *jf,1e-9));
|
||||
EXPECT(assert_equal(expected, *jf,1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -362,15 +358,15 @@ TEST(ExpressionFactor, compose2) {
|
|||
// Check unwhitenedError
|
||||
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_LONGS_EQUAL(1, H.size())
|
||||
EXPECT(assert_equal(2*I_3x3, H[0],1e-9))
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
|
||||
std::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
std::shared_ptr<JacobianFactor> jf = //
|
||||
std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT(assert_equal(expected, *jf,1e-9));
|
||||
EXPECT(assert_equal(expected, *jf,1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -391,15 +387,15 @@ TEST(ExpressionFactor, compose3) {
|
|||
// Check unwhitenedError
|
||||
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_LONGS_EQUAL(1, H.size())
|
||||
EXPECT(assert_equal(I_3x3, H[0],1e-9))
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(3, I_3x3, Z_3x1);
|
||||
std::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
std::shared_ptr<JacobianFactor> jf = //
|
||||
std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT(assert_equal(expected, *jf,1e-9));
|
||||
EXPECT(assert_equal(expected, *jf,1e-9))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -434,17 +430,17 @@ TEST(ExpressionFactor, composeTernary) {
|
|||
// Check unwhitenedError
|
||||
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_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))
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
|
||||
std::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
std::shared_ptr<JacobianFactor> jf = //
|
||||
std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT(assert_equal(expected, *jf,1e-9));
|
||||
EXPECT(assert_equal(expected, *jf,1e-9))
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, tree_finite_differences) {
|
||||
|
|
@ -467,7 +463,7 @@ TEST(ExpressionFactor, tree_finite_differences) {
|
|||
|
||||
const double fd_step = 1e-5;
|
||||
const double tolerance = 1e-5;
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance);
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance)
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, push_back) {
|
||||
|
|
@ -503,8 +499,8 @@ TEST(Expression, testMultipleCompositions) {
|
|||
// Leaf, key = 1
|
||||
// Leaf, key = 2
|
||||
Expression<double> sum1_(Combine(1, 2), v1_, v2_);
|
||||
EXPECT((sum1_.keys() == std::set<Key>{1, 2}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
|
||||
EXPECT((sum1_.keys() == std::set<Key>{1, 2}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance)
|
||||
|
||||
// BinaryExpression(3,4)
|
||||
// BinaryExpression(1,2)
|
||||
|
|
@ -512,8 +508,8 @@ TEST(Expression, testMultipleCompositions) {
|
|||
// Leaf, key = 2
|
||||
// Leaf, key = 1
|
||||
Expression<double> sum2_(Combine(3, 4), sum1_, v1_);
|
||||
EXPECT((sum2_.keys() == std::set<Key>{1, 2}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
|
||||
EXPECT((sum2_.keys() == std::set<Key>{1, 2}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance)
|
||||
|
||||
// BinaryExpression(5,6)
|
||||
// BinaryExpression(3,4)
|
||||
|
|
@ -525,8 +521,8 @@ TEST(Expression, testMultipleCompositions) {
|
|||
// Leaf, key = 1
|
||||
// Leaf, key = 2
|
||||
Expression<double> sum3_(Combine(5, 6), sum1_, sum2_);
|
||||
EXPECT((sum3_.keys() == std::set<Key>{1, 2}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
|
||||
EXPECT((sum3_.keys() == std::set<Key>{1, 2}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -554,20 +550,20 @@ TEST(Expression, testMultipleCompositions2) {
|
|||
Expression<double> v3_(Key(3));
|
||||
|
||||
Expression<double> sum1_(Combine(4,5), v1_, v2_);
|
||||
EXPECT((sum1_.keys() == std::set<Key>{1, 2}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
|
||||
EXPECT((sum1_.keys() == std::set<Key>{1, 2}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance)
|
||||
|
||||
Expression<double> sum2_(combine3, v1_, v2_, v3_);
|
||||
EXPECT((sum2_.keys() == std::set<Key>{1, 2, 3}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
|
||||
EXPECT((sum2_.keys() == std::set<Key>{1, 2, 3}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance)
|
||||
|
||||
Expression<double> sum3_(combine3, v3_, v2_, v1_);
|
||||
EXPECT((sum3_.keys() == std::set<Key>{1, 2, 3}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
|
||||
EXPECT((sum3_.keys() == std::set<Key>{1, 2, 3}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance)
|
||||
|
||||
Expression<double> sum4_(combine3, sum1_, sum2_, sum3_);
|
||||
EXPECT((sum4_.keys() == std::set<Key>{1, 2, 3}));
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance);
|
||||
EXPECT((sum4_.keys() == std::set<Key>{1, 2, 3}))
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -587,7 +583,7 @@ TEST(ExpressionFactor, MultiplyWithInverse) {
|
|||
values.insert<Matrix3>(0, A);
|
||||
values.insert<Vector3>(1, b);
|
||||
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -618,17 +614,17 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
|
|||
Matrix32 H1;
|
||||
Matrix3 A;
|
||||
const Vector Ab = f(a, b, H1, A);
|
||||
CHECK(assert_equal(A * b, Ab));
|
||||
CHECK(assert_equal(A * b, Ab))
|
||||
CHECK(assert_equal(
|
||||
numericalDerivative11<Vector3, Point2>(
|
||||
[&](const Point2& a) { return f(a, b, {}, {}); }, a),
|
||||
H1));
|
||||
H1))
|
||||
|
||||
Values values;
|
||||
values.insert<Point2>(0, a);
|
||||
values.insert<Vector3>(1, b);
|
||||
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -647,7 +643,6 @@ private:
|
|||
public:
|
||||
/// default constructor
|
||||
TestNaryFactor() = default;
|
||||
~TestNaryFactor() override = default;
|
||||
|
||||
TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2,
|
||||
const gtsam::SharedNoiseModel &model, const gtsam::Point3& measured)
|
||||
|
|
@ -723,10 +718,10 @@ TEST(ExpressionFactor, variadicTemplate) {
|
|||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(4);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(4, H.size());
|
||||
EXPECT(assert_equal(Eigen::Vector3d(-5.63578115, -4.85353243, -1.4801204), actual, 1e-5));
|
||||
EXPECT_LONGS_EQUAL(4, H.size())
|
||||
EXPECT(assert_equal(Eigen::Vector3d(-5.63578115, -4.85353243, -1.4801204), actual, 1e-5))
|
||||
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5)
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, normalize) {
|
||||
|
|
@ -740,7 +735,7 @@ TEST(ExpressionFactor, normalize) {
|
|||
Values values;
|
||||
values.insert(1, Vector3(1, 2, 3));
|
||||
ExpressionFactor<Vector3> factor(model, Vector3(1.0/sqrt(14), 2.0/sqrt(14), 3.0/sqrt(14)), f_expr);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, crossProduct) {
|
||||
|
|
@ -756,7 +751,7 @@ TEST(ExpressionFactor, crossProduct) {
|
|||
values.insert(1, Vector3(0.1, 0.2, 0.3));
|
||||
values.insert(2, Vector3(0.4, 0.5, 0.6));
|
||||
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, dotProduct) {
|
||||
|
|
@ -772,7 +767,7 @@ TEST(ExpressionFactor, dotProduct) {
|
|||
values.insert(1, Vector3(0.1, 0.2, 0.3));
|
||||
values.insert(2, Vector3(0.4, 0.5, 0.6));
|
||||
ExpressionFactor<double> factor(model, .0, f_expr);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue