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

Implemented uncalibrate, value test succeeds

release/4.3a0
dellaert 2014-09-21 17:59:34 +02:00
parent 6a5e4191a3
commit 583c81ffea
1 changed files with 21 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
@ -130,27 +129,6 @@ public:
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
Point3 transformTo(const Pose3& x, const Point3& p) {
return x.transform_to(p);
}
Point2 project(const Point3& p) {
return PinholeCamera<Cal3_S2>::project_to_camera(p);
}
/// Expression version of uncalibrate
template<class E1, class E2>
LeafExpression<Point2> uncalibrate(const E1& K, const E2& p) {
return LeafExpression<Point2>(0);
}
/// Expression version of Point2.sub
template<class E1, class E2>
LeafExpression<Point2> operator -(const E1& p, const E2& q) {
return LeafExpression<Point2>(0);
}
/// AD Factor /// AD Factor
template<class T, class E> template<class T, class E>
class BADFactor: NonlinearFactor { class BADFactor: NonlinearFactor {
@ -180,7 +158,7 @@ public:
virtual double error(const Values& values) const { virtual double error(const Values& values) const {
if (this->active(values)) { if (this->active(values)) {
const Vector e = unwhitenedError(values); const Vector e = unwhitenedError(values);
return 0.5 * e.norm(); return 0.5 * e.squaredNorm();
} else { } else {
return 0.0; return 0.0;
} }
@ -209,6 +187,21 @@ public:
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
//-----------------------------------------------------------------------------
Point3 transformTo(const Pose3& x, const Point3& p) {
return x.transform_to(p);
}
Point2 project(const Point3& p) {
return PinholeCamera<Cal3_S2>::project_to_camera(p);
}
template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p) {
return K.uncalibrate(p);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(BAD, test) { TEST(BAD, test) {
@ -220,7 +213,7 @@ TEST(BAD, test) {
values.insert(3, Cal3_S2()); values.insert(3, Cal3_S2());
// Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
Point2 measured(0, 1); Point2 measured(-17, 30);
SharedNoiseModel model = noiseModel::Unit::Create(2); SharedNoiseModel model = noiseModel::Unit::Create(2);
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3); GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values); double expected_error = old.error(values);
@ -237,10 +230,12 @@ TEST(BAD, test) {
typedef UnaryExpression<Point2, Binary1> Unary1; typedef UnaryExpression<Point2, Binary1> Unary1;
Unary1 projection(project, p_cam); Unary1 projection(project, p_cam);
LeafExpression<Point2> uv_hat = uncalibrate(K, projection);
typedef BinaryExpression<Point2, LeafExpression<Cal3_S2>, Unary1> Binary2;
Binary2 uv_hat(uncalibrate, K, projection);
// Create factor // Create factor
BADFactor<Point2, LeafExpression<Point2> > f(measured, uv_hat); BADFactor<Point2, Binary2> f(measured, uv_hat);
// Check value // Check value
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);