Ternary now fixed
dellaert
parent
8b37da54c9
commit
83d77271d9
|
|
@ -310,7 +310,7 @@ class UnaryExpression: public ExpressionNode<T> {
|
|||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double,T::dimension,A::dimension> JacobianTA;
|
||||
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
||||
typedef boost::function<T(const A&, boost::optional<JacobianTA&>)> Function;
|
||||
|
||||
private:
|
||||
|
|
@ -383,8 +383,8 @@ class BinaryExpression: public ExpressionNode<T> {
|
|||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double,T::dimension,A1::dimension> JacobianTA1;
|
||||
typedef Eigen::Matrix<double,T::dimension,A2::dimension> JacobianTA2;
|
||||
typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
|
||||
typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
|
||||
typedef boost::function<
|
||||
T(const A1&, const A2&, boost::optional<JacobianTA1&>,
|
||||
boost::optional<JacobianTA2&>)> Function;
|
||||
|
|
@ -476,9 +476,12 @@ class TernaryExpression: public ExpressionNode<T> {
|
|||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
|
||||
typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
|
||||
typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3;
|
||||
typedef boost::function<
|
||||
T(const A1&, const A2&, const A3&, boost::optional<Matrix&>,
|
||||
boost::optional<Matrix&>, boost::optional<Matrix&>)> Function;
|
||||
T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>,
|
||||
boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function;
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -528,11 +531,13 @@ public:
|
|||
Augmented<A1> a1 = this->expressionA1_->forward(values);
|
||||
Augmented<A2> a2 = this->expressionA2_->forward(values);
|
||||
Augmented<A3> a3 = this->expressionA3_->forward(values);
|
||||
Matrix dTdA1, dTdA2, dTdA3;
|
||||
JacobianTA1 dTdA1;
|
||||
JacobianTA2 dTdA2;
|
||||
JacobianTA3 dTdA3;
|
||||
T t = function_(a1.value(), a2.value(), a3.value(),
|
||||
a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
|
||||
a2.constant() ? none : boost::optional<Matrix&>(dTdA2),
|
||||
a3.constant() ? none : boost::optional<Matrix&>(dTdA3));
|
||||
a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
|
||||
a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2),
|
||||
a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3));
|
||||
return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3,
|
||||
a3.jacobians());
|
||||
}
|
||||
|
|
@ -542,7 +547,9 @@ public:
|
|||
boost::shared_ptr<JacobianTrace<A1> > trace1;
|
||||
boost::shared_ptr<JacobianTrace<A2> > trace2;
|
||||
boost::shared_ptr<JacobianTrace<A3> > trace3;
|
||||
Matrix dTdA1, dTdA2, dTdA3;
|
||||
JacobianTA1 dTdA1;
|
||||
JacobianTA2 dTdA2;
|
||||
JacobianTA3 dTdA3;
|
||||
/// Start the reverse AD process
|
||||
virtual void reverseAD(JacobianMap& jacobians) const {
|
||||
trace1->reverseAD(dTdA1, jacobians);
|
||||
|
|
|
|||
|
|
@ -58,162 +58,217 @@ TEST(BADFactor, test) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Unary(Binary(Leaf,Leaf))
|
||||
TEST(BADFactor, test1) {
|
||||
// Unary(Binary(Leaf,Leaf))
|
||||
TEST(BADFactor, test1) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Pose3());
|
||||
values.insert(2, Point3(0, 0, 1));
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Pose3());
|
||||
values.insert(2, Point3(0, 0, 1));
|
||||
|
||||
// Create old-style factor to create expected value and derivatives
|
||||
GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
|
||||
boost::make_shared<Cal3_S2>());
|
||||
double expected_error = old.error(values);
|
||||
GaussianFactor::shared_ptr expected = old.linearize(values);
|
||||
// Create old-style factor to create expected value and derivatives
|
||||
GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
|
||||
boost::make_shared<Cal3_S2>());
|
||||
double expected_error = old.error(values);
|
||||
GaussianFactor::shared_ptr expected = old.linearize(values);
|
||||
|
||||
// Create leaves
|
||||
Pose3_ x(1);
|
||||
Point3_ p(2);
|
||||
// Create leaves
|
||||
Pose3_ x(1);
|
||||
Point3_ p(2);
|
||||
|
||||
// Try concise version
|
||||
BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf2, 1e-9));
|
||||
}
|
||||
// Try concise version
|
||||
BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
|
||||
TEST(BADFactor, test2) {
|
||||
/* ************************************************************************* */
|
||||
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
|
||||
TEST(BADFactor, test2) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Pose3());
|
||||
values.insert(2, Point3(0, 0, 1));
|
||||
values.insert(3, Cal3_S2());
|
||||
// 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
|
||||
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
|
||||
double expected_error = old.error(values);
|
||||
GaussianFactor::shared_ptr expected = old.linearize(values);
|
||||
// Create old-style factor to create expected value and derivatives
|
||||
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
|
||||
double expected_error = old.error(values);
|
||||
GaussianFactor::shared_ptr expected = old.linearize(values);
|
||||
|
||||
// Create leaves
|
||||
Pose3_ x(1);
|
||||
Point3_ p(2);
|
||||
Cal3_S2_ K(3);
|
||||
// Create leaves
|
||||
Pose3_ x(1);
|
||||
Point3_ p(2);
|
||||
Cal3_S2_ K(3);
|
||||
|
||||
// Create expression tree
|
||||
Point3_ p_cam(x, &Pose3::transform_to, p);
|
||||
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
|
||||
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
|
||||
// Create expression tree
|
||||
Point3_ p_cam(x, &Pose3::transform_to, p);
|
||||
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
|
||||
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
|
||||
|
||||
// Create factor and check value, dimension, linearization
|
||||
BADFactor<Point2> f(model, measured, uv_hat);
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf, 1e-9));
|
||||
// Create factor and check value, dimension, linearization
|
||||
BADFactor<Point2> f(model, measured, uv_hat);
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf, 1e-9));
|
||||
|
||||
// Try concise version
|
||||
BADFactor<Point2> f2(model, measured,
|
||||
uncalibrate(K, project(transform_to(x, p))));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf2, 1e-9));
|
||||
}
|
||||
// Try concise version
|
||||
BADFactor<Point2> f2(model, measured,
|
||||
uncalibrate(K, project(transform_to(x, p))));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf2, 1e-9));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TernaryExpression<Point2,Pose3,Point3,Cal3_S2>::Function fff = project6;
|
||||
|
||||
TEST(BADFactor, compose1) {
|
||||
// Try ternary version
|
||||
BADFactor<Point2> f3(model, measured, project3(x, p, K));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f3.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf3, 1e-9));
|
||||
}
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(2);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
/* ************************************************************************* */
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
TEST(BADFactor, compose1) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
values.insert(2, Rot3());
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(2);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(2);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
values.insert(2, Rot3());
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with arguments referring to the same rotation
|
||||
TEST(BADFactor, compose2) {
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(2);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(1);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
/* ************************************************************************* */
|
||||
// Test compose with arguments referring to the same rotation
|
||||
TEST(BADFactor, compose2) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(1);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, 2 * eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with one arguments referring to a constant same rotation
|
||||
TEST(BADFactor, compose3) {
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, 2 * eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
/* ************************************************************************* */
|
||||
// Test compose with one arguments referring to a constant same rotation
|
||||
TEST(BADFactor, compose3) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(3, Rot3());
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(3, eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(3, Rot3());
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(3, eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with three arguments
|
||||
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
|
||||
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
|
||||
boost::optional<Matrix3&> H3) {
|
||||
// return dummy derivatives (not correct, but that's ok for testing here)
|
||||
if (H1)
|
||||
*H1 = eye(3);
|
||||
if (H2)
|
||||
*H2 = eye(3);
|
||||
if (H3)
|
||||
*H3 = eye(3);
|
||||
return R1 * (R2 * R3);
|
||||
}
|
||||
|
||||
TEST(BADFactor, composeTernary) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ A(1), B(2), C(3);
|
||||
Rot3_ ABC(composeThree, A, B, C);
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
values.insert(2, Rot3());
|
||||
values.insert(3, Rot3());
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(3);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(3, H.size());
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[2],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#include <gtsam_unstable/nonlinear/Expression.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) {
|
|||
return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
|
||||
}
|
||||
|
||||
Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
|
||||
boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
|
||||
boost::optional<Matrix25&> Dcal) {
|
||||
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
|
||||
}
|
||||
|
||||
Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
|
||||
return Point2_(project6, x, p, K);
|
||||
}
|
||||
|
||||
template<class CAL>
|
||||
Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
|
||||
return Point2_(K, &CAL::uncalibrate, xy_hat);
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
#include <time.h>
|
||||
#include <iostream>
|
||||
#include <iomanip> // std::setprecision
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
|
@ -74,37 +73,43 @@ int main() {
|
|||
// Dedicated factor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 4.2 musecs/call
|
||||
GeneralSFMFactor2<Cal3_S2> oldFactor2(z, model, 1, 2, 3);
|
||||
time(oldFactor2, values);
|
||||
GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
|
||||
time(f1, values);
|
||||
|
||||
// BADFactor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 20.3 musecs/call
|
||||
BADFactor<Point2> newFactor2(model, z,
|
||||
BADFactor<Point2> f2(model, z,
|
||||
uncalibrate(K, project(transform_to(x, p))));
|
||||
time(newFactor2, values);
|
||||
time(f2, values);
|
||||
|
||||
// BADFactor ternary
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 20.3 musecs/call
|
||||
BADFactor<Point2> f3(model, z, project3(x, p, K));
|
||||
time(f3, values);
|
||||
|
||||
// CALIBRATED
|
||||
|
||||
// Dedicated factor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 3.4 musecs/call
|
||||
GenericProjectionFactor<Pose3, Point3> oldFactor1(z, model, 1, 2, fixedK);
|
||||
time(oldFactor1, values);
|
||||
GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
|
||||
time(g1, values);
|
||||
|
||||
// BADFactor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 16.0 musecs/call
|
||||
BADFactor<Point2> newFactor1(model, z,
|
||||
BADFactor<Point2> g2(model, z,
|
||||
uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
|
||||
time(newFactor1, values);
|
||||
time(g2, values);
|
||||
|
||||
// BADFactor, optimized
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 9.0 musecs/call
|
||||
typedef PinholeCamera<Cal3_S2> Camera;
|
||||
typedef Expression<Camera> Camera_;
|
||||
BADFactor<Point2> newFactor3(model, z, Point2_(myProject, x, p));
|
||||
time(newFactor3, values);
|
||||
BADFactor<Point2> g3(model, z, Point2_(myProject, x, p));
|
||||
time(g3, values);
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue