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

Now use old-style factor to create expected value and derivatives

release/4.3a0
dellaert 2014-09-21 05:33:53 -04:00
parent a76c27d074
commit c7b6a9af12
1 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
@ -101,11 +102,23 @@ public:
TEST(BAD, test) {
// Create some values
Values values;
values.insert(1,Pose3());
values.insert(2,Point3(0,0,1));
values.insert(3,Cal3_S2());
// Create old-style factor to create expected value and derivatives
Point2 measured(0,1);
SharedNoiseModel model = noiseModel::Unit::Create(2);
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create leaves
Expression<Pose3> x(1);
Expression<Point3> p(2);
Expression<Cal3_S2> K(3);
Expression<Point2> uv(Point2(300, 62));
Expression<Point2> uv(measured);
// Create expression tree
Expression<Point3> p_cam = transformTo(x, p);
@ -116,25 +129,15 @@ TEST(BAD, test) {
// Create factor
BADFactor<Point2> f(e);
// Create some values
Values values;
// Check value
EXPECT_DOUBLES_EQUAL(0, f.error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
// Check dimension
EXPECT_LONGS_EQUAL(0, f.dim());
// Check linearization
JacobianFactor expected( //
1, (Matrix(2, 6) << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12), //
2, (Matrix(2, 3) << 1, 2, 3, 4, 5, 6), //
3, (Matrix(2, 5) << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10), //
(Vector(2) << 1, 2));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
EXPECT( assert_equal(*expected, *gf, 1e-9));
}
/* ************************************************************************* */