Fixed some unit tests
parent
e2733d9899
commit
0212bbc30d
|
@ -184,7 +184,9 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
|
||||||
VectorValues actual = gbn.optimizeGradientSearch();
|
VectorValues actual = gbn.optimizeGradientSearch();
|
||||||
|
|
||||||
// Check that points agree
|
// Check that points agree
|
||||||
EXPECT(assert_equal(expected, actual.vector(), 1e-5));
|
Vector actualAsVector = actual.vector(FastVector<Key>(list_of
|
||||||
|
(0)(1)(2)(3)(4)));
|
||||||
|
EXPECT(assert_equal(expected, actualAsVector, 1e-5));
|
||||||
|
|
||||||
// Check that point causes a decrease in error
|
// Check that point causes a decrease in error
|
||||||
double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual));
|
double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual));
|
||||||
|
|
|
@ -274,7 +274,9 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
|
||||||
VectorValues actual = bt.optimizeGradientSearch();
|
VectorValues actual = bt.optimizeGradientSearch();
|
||||||
|
|
||||||
// Check that points agree
|
// Check that points agree
|
||||||
EXPECT(assert_equal(expected, actual.vector(), 1e-5));
|
Vector actualAsVector = actual.vector(FastVector<Key>(list_of
|
||||||
|
(0)(1)(2)(3)(4)));
|
||||||
|
EXPECT(assert_equal(expected, actualAsVector, 1e-5));
|
||||||
EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
|
EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
|
||||||
|
|
||||||
// Check that point causes a decrease in error
|
// Check that point causes a decrease in error
|
||||||
|
|
|
@ -16,6 +16,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <boost/assign/std/vector.hpp>
|
#include <boost/assign/std/vector.hpp>
|
||||||
|
#include <boost/assign/list_of.hpp>
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
|
@ -59,7 +60,8 @@ TEST(VectorValues, basics)
|
||||||
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
|
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
|
||||||
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
|
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
|
||||||
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
|
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
|
||||||
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
|
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector(FastVector<Key>(list_of
|
||||||
|
(0)(1)(2)(5)))));
|
||||||
|
|
||||||
// Check exceptions
|
// Check exceptions
|
||||||
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
|
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
|
||||||
|
|
|
@ -55,12 +55,14 @@ GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector subB = jf->getb().segment(startRow, nrRows);
|
Vector subB = jf->getb().segment(startRow, nrRows);
|
||||||
Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows);
|
|
||||||
SharedDiagonal model;
|
SharedDiagonal model;
|
||||||
if (jf->get_model()->isConstrained())
|
if(jf->get_model()) {
|
||||||
model = noiseModel::Constrained::MixedSigmas(sigmas);
|
Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows);
|
||||||
else
|
if (jf->get_model()->isConstrained())
|
||||||
model = noiseModel::Diagonal::Sigmas(sigmas);
|
model = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||||
|
else
|
||||||
|
model = noiseModel::Diagonal::Sigmas(sigmas);
|
||||||
|
}
|
||||||
|
|
||||||
// extract matrices from each
|
// extract matrices from each
|
||||||
// assemble into new JacobianFactor
|
// assemble into new JacobianFactor
|
||||||
|
|
|
@ -277,7 +277,6 @@ TEST( testBayesTreeOperations, liquefy ) {
|
||||||
|
|
||||||
// Liquefy the tree back into a graph, splitting factors
|
// Liquefy the tree back into a graph, splitting factors
|
||||||
{
|
{
|
||||||
CHECK(("*** liquify fails here *** - does not check for null noiseModel", 0));
|
|
||||||
GaussianFactorGraph actGraph = liquefy(bayesTree, true);
|
GaussianFactorGraph actGraph = liquefy(bayesTree, true);
|
||||||
GaussianFactorGraph expGraph;
|
GaussianFactorGraph expGraph;
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@ set (slam_full_libs
|
||||||
|
|
||||||
# Exclude tests that don't work
|
# Exclude tests that don't work
|
||||||
set (slam_excluded_tests
|
set (slam_excluded_tests
|
||||||
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
|
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
|
||||||
# "" # Add to this list, with full path, to exclude
|
# "" # Add to this list, with full path, to exclude
|
||||||
)
|
)
|
||||||
# Add all tests
|
# Add all tests
|
||||||
|
|
|
@ -10,13 +10,6 @@
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
TEST(testSerialization, disabled)
|
|
||||||
{
|
|
||||||
CHECK(("*** testSerialization in gtsam_unstable/slam is disabled *** - Needs conversion for unordered", 0));
|
|
||||||
}
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
|
|
||||||
#include <gtsam/slam/serialization.h>
|
#include <gtsam/slam/serialization.h>
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
@ -125,8 +118,6 @@ TEST( testSerialization, serialization_file ) {
|
||||||
EXPECT(assert_equal(values, actValuesXML));
|
EXPECT(assert_equal(values, actValuesXML));
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue