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

Added more powerful tests on updateHessian

release/4.3a0
dellaert 2015-06-14 12:53:20 -07:00
parent 850501ed52
commit e045a5e1f7
1 changed files with 34 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/RangeFactor.h> #include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -28,9 +29,10 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/assign/std/vector.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace boost; using namespace boost::assign;
#include <iostream> #include <iostream>
#include <vector> #include <vector>
@ -441,7 +443,8 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GeneralSFMFactor, Linearize) { // Frank created these tests after switching to a custom LinearizedFactor
TEST(GeneralSFMFactor, LinearizedFactor) {
Point2 z(3., 0.); Point2 z(3., 0.);
// Create Values // Create Values
@ -453,44 +456,42 @@ TEST(GeneralSFMFactor, Linearize) {
Point3 l1; Point3 l1;
values.insert(L(1), l1); values.insert(L(1), l1);
// Test with Empty Model vector<SharedNoiseModel> models;
{ {
const SharedNoiseModel model; // Create various noise-models to test all cases
Projection factor(z, model, X(1), L(1)); using namespace noiseModel;
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( Rot2 R = Rot2::fromAngle(0.3);
values); Matrix2 cov = R.matrix() * R.matrix().transpose();
GaussianFactor::shared_ptr actual = factor.linearize(values); models += SharedNoiseModel(), Unit::Create(2), //
EXPECT(assert_equal(*expected, *actual, 1e-9)); Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov);
} }
// Test with Unit Model
{ // Now loop over all these noise models
const SharedNoiseModel model(noiseModel::Unit::Create(2)); BOOST_FOREACH(SharedNoiseModel model, models) {
Projection factor(z, model, X(1), L(1)); Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values); // Test linearize
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr expected = //
EXPECT(assert_equal(*expected, *actual, 1e-9)); factor.NoiseModelFactor::linearize(values);
}
// Test with Isotropic noise
{
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
}
// Test with Constrained Model
{
const SharedNoiseModel model(noiseModel::Constrained::All(2));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9)); EXPECT(assert_equal(*expected, *actual, 1e-9));
// Test methods that rely on updateHessian
if (model && !model->isConstrained()) {
// Construct HessianFactor from single JacobianFactor
HessianFactor expectedHessian(*expected), actualHessian(*actual);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9));
// Construct from GaussianFactorGraph
GaussianFactorGraph gfg1;
gfg1 += expected;
GaussianFactorGraph gfg2;
gfg2 += actual;
HessianFactor hessian1(gfg1), hessian2(gfg2);
EXPECT(assert_equal(hessian1, hessian2, 1e-9));
}
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;