Frank Dellaert
GitHub
commit
03de47b860
|
|
@ -19,15 +19,15 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
// The MSVC compiler workaround for the unsupported variable length array
|
||||
// utilizes the std::unique_ptr<> custom deleter.
|
||||
// See Expression<T>::valueAndJacobianMap() below.
|
||||
#ifdef _MSC_VER
|
||||
#include <memory>
|
||||
#endif
|
||||
|
||||
#include <gtsam/nonlinear/internal/ExpressionNode.h>
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<typename T>
|
||||
|
|
@ -145,9 +145,10 @@ T Expression<T>::value(const Values& values,
|
|||
// Call private version that returns derivatives in H
|
||||
const auto [keys, dims] = keysAndDims();
|
||||
return valueAndDerivatives(values, keys, dims, *H);
|
||||
} else
|
||||
} else {
|
||||
// no derivatives needed, just return value
|
||||
return root_->value(values);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
|
|
@ -188,38 +189,39 @@ T Expression<T>::valueAndDerivatives(const Values& values,
|
|||
|
||||
template<typename T>
|
||||
T Expression<T>::traceExecution(const Values& values,
|
||||
internal::ExecutionTrace<T>& trace, void* traceStorage) const {
|
||||
return root_->traceExecution(values, trace,
|
||||
static_cast<internal::ExecutionTraceStorage*>(traceStorage));
|
||||
internal::ExecutionTrace<T>& trace, char* traceStorage) const {
|
||||
return root_->traceExecution(values, trace, traceStorage);
|
||||
}
|
||||
|
||||
// Allocate a single block of aligned memory using a unique_ptr.
|
||||
inline std::unique_ptr<internal::ExecutionTraceStorage[]> allocAligned(size_t size) {
|
||||
const size_t alignedSize = (size + internal::TraceAlignment - 1) / internal::TraceAlignment;
|
||||
return std::unique_ptr<internal::ExecutionTraceStorage[]>(
|
||||
new internal::ExecutionTraceStorage[alignedSize]);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T Expression<T>::valueAndJacobianMap(const Values& values,
|
||||
internal::JacobianMap& jacobians) const {
|
||||
// The following piece of code is absolutely crucial for performance.
|
||||
// We allocate a block of memory on the stack, which can be done at runtime
|
||||
// with modern C++ compilers. The traceExecution then fills this memory
|
||||
// with an execution trace, made up entirely of "Record" structs, see
|
||||
// the FunctionalNode class in expression-inl.h
|
||||
size_t size = traceSize();
|
||||
try {
|
||||
// We allocate a single block of aligned memory using a unique_ptr.
|
||||
const size_t size = traceSize();
|
||||
auto traceStorage = allocAligned(size);
|
||||
|
||||
// Windows does not support variable length arrays, so memory must be dynamically
|
||||
// allocated on Visual Studio. For more information see the issue below
|
||||
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
|
||||
#ifdef _MSC_VER
|
||||
std::unique_ptr<void, void(*)(void*)> traceStorageDeleter(
|
||||
_aligned_malloc(size, internal::TraceAlignment),
|
||||
[](void *ptr){ _aligned_free(ptr); });
|
||||
auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(traceStorageDeleter.get());
|
||||
#else
|
||||
internal::ExecutionTraceStorage traceStorage[size];
|
||||
#endif
|
||||
// The traceExecution call then fills this memory
|
||||
// with an execution trace, made up entirely of "Record" structs, see
|
||||
// the FunctionalNode class in expression-inl.h
|
||||
internal::ExecutionTrace<T> trace;
|
||||
T value(this->traceExecution(values, trace, reinterpret_cast<char *>(traceStorage.get())));
|
||||
|
||||
internal::ExecutionTrace<T> trace;
|
||||
T value(this->traceExecution(values, trace, traceStorage));
|
||||
trace.startReverseAD1(jacobians);
|
||||
|
||||
return value;
|
||||
// We then calculate the Jacobians using reverse automatic differentiation (AD).
|
||||
trace.startReverseAD1(jacobians);
|
||||
return value;
|
||||
} catch (const std::bad_alloc &e) {
|
||||
std::cerr << "valueAndJacobianMap exception: " << e.what() << '\n';
|
||||
throw e;
|
||||
}
|
||||
// Here traceStorage will be de-allocated properly.
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
|
|
@ -261,7 +263,7 @@ struct apply_compose<double> {
|
|||
}
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace internal
|
||||
|
||||
// Global methods:
|
||||
|
||||
|
|
|
|||
|
|
@ -197,7 +197,7 @@ protected:
|
|||
|
||||
/// trace execution, very unsafe
|
||||
T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
|
||||
void* traceStorage) const;
|
||||
char* traceStorage) const;
|
||||
|
||||
/// brief Return value and derivatives, reverse AD version
|
||||
T valueAndJacobianMap(const Values& values,
|
||||
|
|
|
|||
|
|
@ -24,7 +24,9 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/nonlinear/Expression.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
#include <numeric>
|
||||
#include <utility>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -147,7 +149,7 @@ protected:
|
|||
noiseModel_->WhitenSystem(Ab.matrix(), b);
|
||||
}
|
||||
|
||||
return factor;
|
||||
return std::move(factor);
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
|
|
@ -252,9 +254,6 @@ public:
|
|||
// Provide access to the Matrix& version of unwhitenedError:
|
||||
using ExpressionFactor<T>::unwhitenedError;
|
||||
|
||||
/// Destructor
|
||||
~ExpressionFactorN() override = default;
|
||||
|
||||
// Don't provide backward compatible evaluateVector(), due to its problematic
|
||||
// variable length of optional Jacobian arguments. Vector evaluateError(const
|
||||
// Args... args,...);
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
|
||||
#include <iostream>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -37,7 +38,13 @@ template<int T> struct CallRecord;
|
|||
/// Storage type for the execution trace.
|
||||
/// It enforces the proper alignment in a portable way.
|
||||
/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
|
||||
static const unsigned TraceAlignment = 16; // 16 bytes is the default alignment used by Eigen.
|
||||
#ifdef _MSC_VER
|
||||
// TODO(dellaert): this might lead to trouble if Eigen decides to use 32 on Windows.
|
||||
static const unsigned TraceAlignment = 16; // 16 bytes max_align on Windows
|
||||
#else
|
||||
static const unsigned TraceAlignment = 32; // Alignment used by Eigen on some platforms.
|
||||
#endif
|
||||
// TODO(dellaert): we *should* be able to simplify the code using the pointer arithmetic from ExecutionTraceStorage.
|
||||
typedef std::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
|
||||
|
||||
template<bool UseBlock, typename Derived>
|
||||
|
|
@ -123,11 +130,11 @@ class ExecutionTrace {
|
|||
|
||||
/// Print
|
||||
void print(const std::string& indent = "") const {
|
||||
if (kind == Constant)
|
||||
if (kind == Constant) {
|
||||
std::cout << indent << "Constant" << std::endl;
|
||||
else if (kind == Leaf)
|
||||
} else if (kind == Leaf) {
|
||||
std::cout << indent << "Leaf, key = " << content.key << std::endl;
|
||||
else if (kind == Function) {
|
||||
} else if (kind == Function) {
|
||||
content.ptr->print(indent + " ");
|
||||
}
|
||||
}
|
||||
|
|
@ -135,9 +142,9 @@ class ExecutionTrace {
|
|||
/// Return record pointer, quite unsafe, used only for testing
|
||||
template<class Record>
|
||||
std::optional<Record*> record() {
|
||||
if (kind != Function)
|
||||
if (kind != Function) {
|
||||
return {};
|
||||
else {
|
||||
} else {
|
||||
Record* p = dynamic_cast<Record*>(content.ptr);
|
||||
return p ? std::optional<Record*>(p) : std::nullopt;
|
||||
}
|
||||
|
|
@ -152,10 +159,11 @@ class ExecutionTrace {
|
|||
// This branch will only be called on trivial Leaf expressions, i.e. Priors
|
||||
static const JacobianTT I = JacobianTT::Identity();
|
||||
handleLeafCase(I, jacobians, content.key);
|
||||
} else if (kind == Function)
|
||||
} else if (kind == Function) {
|
||||
// This is the more typical entry point, starting the AD pipeline
|
||||
// Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
|
||||
content.ptr->startReverseAD2(jacobians);
|
||||
}
|
||||
}
|
||||
|
||||
/// Either add to Jacobians (Leaf) or propagate (Function)
|
||||
|
|
|
|||
|
|
@ -110,7 +110,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD
|
||||
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* traceStorage) const = 0;
|
||||
char* traceStorage) const = 0;
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
|
@ -146,7 +146,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* traceStorage) const override {
|
||||
char* traceStorage) const override {
|
||||
return constant_;
|
||||
}
|
||||
|
||||
|
|
@ -199,7 +199,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* traceStorage) const override {
|
||||
char* traceStorage) const override {
|
||||
trace.setLeaf(key_);
|
||||
return values.at<T>(key_);
|
||||
}
|
||||
|
|
@ -276,7 +276,7 @@ public:
|
|||
A1 value1;
|
||||
|
||||
/// Construct record by calling argument expression
|
||||
Record(const Values& values, const ExpressionNode<A1>& expression1, ExecutionTraceStorage* ptr)
|
||||
Record(const Values& values, const ExpressionNode<A1>& expression1, char* ptr)
|
||||
: value1(expression1.traceExecution(values, trace1, ptr + upAligned(sizeof(Record)))) {}
|
||||
|
||||
/// Print to std::cout
|
||||
|
|
@ -308,7 +308,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* ptr) const override {
|
||||
char* ptr) const override {
|
||||
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
|
||||
|
||||
// Create a Record in the memory pointed to by ptr
|
||||
|
|
@ -399,7 +399,7 @@ public:
|
|||
|
||||
/// Construct record by calling argument expressions
|
||||
Record(const Values& values, const ExpressionNode<A1>& expression1,
|
||||
const ExpressionNode<A2>& expression2, ExecutionTraceStorage* ptr)
|
||||
const ExpressionNode<A2>& expression2, char* ptr)
|
||||
: value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
|
||||
value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())) {}
|
||||
|
||||
|
|
@ -427,7 +427,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* ptr) const override {
|
||||
char* ptr) const override {
|
||||
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
|
||||
Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
|
||||
trace.setFunction(record);
|
||||
|
|
@ -498,6 +498,8 @@ public:
|
|||
// Inner Record Class
|
||||
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
typename Jacobian<T, A1>::type dTdA1;
|
||||
typename Jacobian<T, A2>::type dTdA2;
|
||||
typename Jacobian<T, A3>::type dTdA3;
|
||||
|
|
@ -513,7 +515,7 @@ public:
|
|||
/// Construct record by calling 3 argument expressions
|
||||
Record(const Values& values, const ExpressionNode<A1>& expression1,
|
||||
const ExpressionNode<A2>& expression2,
|
||||
const ExpressionNode<A3>& expression3, ExecutionTraceStorage* ptr)
|
||||
const ExpressionNode<A3>& expression3, char* ptr)
|
||||
: value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
|
||||
value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())),
|
||||
value3(expression3.traceExecution(values, trace3, ptr += expression2.traceSize())) {}
|
||||
|
|
@ -545,7 +547,7 @@ public:
|
|||
|
||||
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* ptr) const override {
|
||||
char* ptr) const override {
|
||||
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
|
||||
Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
|
||||
trace.setFunction(record);
|
||||
|
|
@ -625,7 +627,7 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
|
|||
|
||||
/// Construct an execution trace for reverse AD
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* ptr) const override {
|
||||
char* ptr) const override {
|
||||
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
|
||||
Record* record = new (ptr) Record();
|
||||
ptr += upAligned(sizeof(Record));
|
||||
|
|
@ -717,14 +719,14 @@ class BinarySumNode : public ExpressionNode<T> {
|
|||
};
|
||||
|
||||
/// Construct an execution trace for reverse AD
|
||||
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
|
||||
ExecutionTraceStorage* ptr) const override {
|
||||
T traceExecution(const Values &values, ExecutionTrace<T> &trace,
|
||||
char* ptr) const override {
|
||||
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
|
||||
Record* record = new (ptr) Record();
|
||||
Record *record = new (ptr) Record();
|
||||
trace.setFunction(record);
|
||||
|
||||
ExecutionTraceStorage* ptr1 = ptr + upAligned(sizeof(Record));
|
||||
ExecutionTraceStorage* ptr2 = ptr1 + expression1_->traceSize();
|
||||
auto ptr1 = ptr + upAligned(sizeof(Record));
|
||||
auto ptr2 = ptr1 + expression1_->traceSize();
|
||||
return expression1_->traceExecution(values, record->trace1, ptr1) +
|
||||
expression2_->traceExecution(values, record->trace2, ptr2);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
auto traceStorage = allocAligned(binary.traceSize());
|
||||
internal::ExecutionTrace<Point2> trace;
|
||||
Point2 value = binary.traceExecution(values, trace, traceStorage);
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
|
||||
Point2 value = binary.traceExecution(values, trace, reinterpret_cast<char *>(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];
|
||||
auto traceStorage = allocAligned(expression.traceSize());
|
||||
internal::ExecutionTrace<Point2> trace;
|
||||
Point2 value = expression.traceExecution(values, trace, traceStorage);
|
||||
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
|
||||
Point2 value = expression.traceExecution(values, trace, reinterpret_cast<char *>(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