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

Fixed dimensions of Vectors

release/4.3a0
dellaert 2014-11-04 15:41:14 +01:00
parent 8b86626113
commit 4afe132b1a
10 changed files with 101 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -28,20 +28,20 @@ public:
protected:
/** measurements from the IMU */
Vector accel_, gyro_;
Vector3 accel_, gyro_;
double dt_; /// time between measurements
public:
/** Standard constructor */
FullIMUFactor(const Vector& accel, const Vector& gyro,
FullIMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
assert(model->dim() == 9);
}
/** Single IMU vector - imu = [accel, gyro] */
FullIMUFactor(const Vector& imu,
FullIMUFactor(const Vector6& imu,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
assert(imu.size() == 6);
@ -67,15 +67,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "FullIMUFactor: " + s;
Base::print(a, formatter);
gtsam::print(accel_, "accel");
gtsam::print(gyro_, "gyro");
gtsam::print((Vector)accel_, "accel");
gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl;
}
// access
const Vector& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); }
const Vector3& gyro() const { return gyro_; }
const Vector3& accel() const { return accel_; }
Vector6 z() const { return (Vector6() << accel_, gyro_); }
/**
* Error evaluation with optional derivatives - calculates
@ -84,13 +84,13 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector z(9);
Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<Vector3, PoseRTV, PoseRTV>(
if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector3, PoseRTV, PoseRTV>(
if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
return z - predict_proxy(x1, x2, dt_);
}
@ -106,11 +106,11 @@ public:
private:
/** copy of the measurement function formulated for numerical derivatives */
static Vector3 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector hx(9);
static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
return Vector3(hx);
return hx;
}
};

30
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -26,18 +26,18 @@ public:
protected:
/** measurements from the IMU */
Vector accel_, gyro_;
Vector3 accel_, gyro_;
double dt_; /// time between measurements
public:
/** Standard constructor */
IMUFactor(const Vector& accel, const Vector& gyro,
IMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
/** Full IMU vector specification */
IMUFactor(const Vector& imu_vector,
IMUFactor(const Vector6& imu_vector,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
@ -60,15 +60,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "IMUFactor: " + s;
Base::print(a, formatter);
gtsam::print(accel_, "accel");
gtsam::print(gyro_, "gyro");
gtsam::print((Vector)accel_, "accel");
gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl;
}
// access
const Vector& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); }
const Vector3& gyro() const { return gyro_; }
const Vector3& accel() const { return accel_; }
Vector6 z() const { return (Vector6() << accel_, gyro_);}
/**
* Error evaluation with optional derivatives - calculates
@ -77,10 +77,10 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z();
if (H1) *H1 = numericalDerivative21<Vector, PoseRTV, PoseRTV>(
const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector, PoseRTV, PoseRTV>(
if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
return predict_proxy(x1, x2, dt_, meas);
}
@ -95,10 +95,10 @@ public:
private:
/** copy of the measurement function formulated for numerical derivatives */
static Vector predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector& meas) {
Vector hx = x1.imuPrediction(x2, dt);
return Vector(meas - hx);
static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector6& meas) {
Vector6 hx = x1.imuPrediction(x2, dt);
return meas - hx;
}
};

34
gtsam_unstable/dynamics/Pendulum.h
View File

@ -37,7 +37,7 @@ public:
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
: Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -45,15 +45,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(double qk1, double qk, double v,
Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim();
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_;
return qk1.localCoordinates(qk.compose(double(v*h_)));
return (Vector(1) << qk+v*h_-qk1);
}
}; // \PendulumFactor1
@ -85,7 +85,7 @@ public:
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
: Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -93,15 +93,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(double vk1, double vk, double q,
Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim();
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value());
return vk1.localCoordinates(double(vk - h_*g_/r_*sin(q)));
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1);
}
}; // \PendulumFactor2
@ -135,7 +135,7 @@ public:
///Constructor
PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey, qKey, qKey1),
: Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor
@ -144,11 +144,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
Vector evaluateError(double pk, double qk, double qk1,
Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim();
const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_;
@ -158,7 +158,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return pk.localCoordinates(double(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk);
}
}; // \PendulumFactorPk
@ -191,7 +191,7 @@ public:
///Constructor
PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey1, qKey, qKey1),
: Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor
@ -200,11 +200,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
Vector evaluateError(double pk1, double qk, double qk1,
Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim();
const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_;
@ -214,7 +214,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return pk1.localCoordinates(double(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1);
}
}; // \PendulumFactorPk1

29
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const {
}
/* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector& v) {
assert(v.size() == 9);
Pose3 newPose = Pose3::Expmap(sub(v, 0, 6));
Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
PoseRTV PoseRTV::Expmap(const Vector9& v) {
Pose3 newPose = Pose3::Expmap(v.head<6>());
Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
return PoseRTV(newPose, newVel);
}
/* ************************************************************************* */
Vector PoseRTV::Logmap(const PoseRTV& p) {
Vector Lx = Pose3::Logmap(p.Rt_);
Vector Lv = Velocity3::Logmap(p.v_);
return concatVectors(2, &Lx, &Lv);
Vector9 PoseRTV::Logmap(const PoseRTV& p) {
Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector3 Lv = Velocity3::Logmap(p.v_);
return (Vector9() << Lx, Lv);
}
/* ************************************************************************* */
@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation
Vector poseLogmap = x0.localCoordinates(x1);
Vector lv = rotation().unrotate(p1.velocity() - v_).vector();
return concatVectors(2, &poseLogmap, &lv);
Vector6 poseLogmap = x0.localCoordinates(x1);
Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
return (Vector(9) << poseLogmap, lv);
}
/* ************************************************************************* */
@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics(
}
/* ************************************************************************* */
Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// split out states
const Rot3 &r1 = R(), &r2 = x2.R();
const Velocity3 &v1 = v(), &v2 = x2.v();
Vector imu(6);
Vector6 imu;
// acceleration
Vector accel = v1.localCoordinates(v2) / dt;
imu.head(3) = r2.transpose() * (accel - g);
imu.head<3>() = r2.transpose() * (accel - g);
// rotation rates
// just using euler angles based on matlab code
@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// normalize yaw in difference (as per Mitch's code)
dR(2) = Rot2::fromAngle(dR(2)).theta();
dR /= dt;
imu.tail(3) = Enb * dR;
imu.tail<3>() = Enb * dR;
// imu.tail(3) = r1.transpose() * dR;
return imu;

6
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -86,8 +86,8 @@ public:
* expmap/logmap are poor approximations that assume independence of components
* Currently implemented using the poor retract/unretract approximations
*/
static PoseRTV Expmap(const Vector& v);
static Vector Logmap(const PoseRTV& p);
static PoseRTV Expmap(const Vector9& v);
static Vector9 Logmap(const PoseRTV& p);
static PoseRTV identity() { return PoseRTV(); }
@ -129,7 +129,7 @@ public:
/// Dynamics predictor for both ground and flying robots, given states at 1 and 2
/// Always move from time 1 to time 2
/// @return imu measurement, as [accel, gyro]
Vector imuPrediction(const PoseRTV& x2, double dt) const;
Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
/// predict measurement and where Point3 for x2 should be, as a way
/// of enforcing a velocity constraint

8
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -27,7 +27,7 @@ public:
///TODO: comment
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {}
: Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {}
virtual ~VelocityConstraint3() {}
/// @return a deep copy of this factor
@ -36,15 +36,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
/** x1 + v*dt - x2 = 0, with optional derivatives */
Vector evaluateError(double x1, double x2, double v,
Vector evaluateError(const double& x1, const double& x2, const double& v,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim();
const size_t p = 1;
if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_;
return x2.localCoordinates(x1.compose(double(v*dt_)));
return (Vector(1) << x1+v*dt_-x2);
}
private:

14
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create measurements
SharedDiagonal model = noiseModel::Unit::Create(6);
Vector imu12(6), imu23(6), imu34(6);
imu12 = pose1.imuPrediction(pose2, dt);
imu23 = pose2.imuPrediction(pose3, dt);
imu34 = pose3.imuPrediction(pose4, dt);
Vector6 imu12 = pose1.imuPrediction(pose2, dt);
Vector6 imu23 = pose2.imuPrediction(pose3, dt);
Vector6 imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;
@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
Vector imu12(6), imu23(6), imu34(6);
imu12 = pose1.imuPrediction(pose2, dt);
imu23 = pose2.imuPrediction(pose3, dt);
imu34 = pose3.imuPrediction(pose4, dt);
Vector6 imu12 = pose1.imuPrediction(pose2, dt);
Vector6 imu23 = pose2.imuPrediction(pose3, dt);
Vector6 imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;

8
gtsam_unstable/dynamics/tests/testPendulumFactors.cpp
View File

@ -18,8 +18,8 @@ namespace {
const double g = 9.81, l = 1.0;
const double deg2rad = M_PI/180.0;
LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0);
LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
double q1(deg2rad*30.0), q2(deg2rad*31.0);
double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
}
@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) {
// hard constraints don't need a noise model
PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) );
double pk( 1/h * (q2-q1) + h*g*sin(q1) );
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
// hard constraints don't need a noise model
PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
LieScalar pk1( 1/h * (q2-q1) );
double pk1( 1/h * (q2-q1) );
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));

23
gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp
View File

@ -8,23 +8,16 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
/* ************************************************************************* */
using namespace gtsam;
namespace {
const double tol=1e-5;
const double dt = 1.0;
LieScalar origin,
x1(1.0), x2(2.0), v(1.0);
}
using namespace gtsam::symbol_shorthand;
/* ************************************************************************* */
// evaluateError
TEST( testVelocityConstraint3, evaluateError) {
using namespace gtsam::symbol_shorthand;
const double tol = 1e-5;
const double dt = 1.0;
double x1(1.0), x2(2.0), v(1.0);
// hard constraints don't need a noise model
VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) {
EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

30
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4);
double inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
EXPECT_DOUBLES_EQUAL(expected_uv, actual_uv,1e-8);
EXPECT(assert_equal(Point2(640,480), actual_uv));
}
@ -46,7 +46,7 @@ TEST( InvDepthFactor, Project2) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
LieScalar inv_depth(1/sqrt(3.0));
double inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
@ -61,7 +61,7 @@ TEST( InvDepthFactor, Project3) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
LieScalar inv_depth( 1./sqrt(1.0+1+4));
double inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
@ -76,20 +76,20 @@ TEST( InvDepthFactor, Project4) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
LieScalar inv_depth(1./sqrt(1.+16.+4.));
double inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) {
Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) {
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose)
{
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
double inv_depth(1./4);
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
@ -101,7 +101,7 @@ TEST( InvDepthFactor, Dproject_pose)
TEST( InvDepthFactor, Dproject_landmark)
{
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
double inv_depth(1./4);
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
@ -113,7 +113,7 @@ TEST( InvDepthFactor, Dproject_landmark)
TEST( InvDepthFactor, Dproject_inv_depth)
{
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
double inv_depth(1./4);
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
@ -125,15 +125,15 @@ TEST( InvDepthFactor, Dproject_inv_depth)
TEST(InvDepthFactor, backproject)
{
Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4);
double inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
Vector5 actual_vec;
LieScalar actual_inv;
double actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
}
/* ************************************************************************* */
@ -141,15 +141,15 @@ TEST(InvDepthFactor, backproject2)
{
// backwards facing camera
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10);
double inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth);
Vector5 actual_vec;
LieScalar actual_inv;
double actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
}
/* ************************************************************************* */