Frank Dellaert
commit
242b3ef9f0
|
|
@ -58,6 +58,22 @@ Point3 Point3::operator/(double s) const {
|
|||
return Point3(x_ / s, y_ / s, z_ / s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
double d = (p2 - *this).norm();
|
||||
if (H1) {
|
||||
*H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
|
||||
*H1 = *H1 *(1. / d);
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
|
||||
*H2 = *H2 *(1. / d);
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
|
||||
OptionalJacobian<3,3> H2) const {
|
||||
|
|
@ -75,13 +91,27 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::cross(const Point3 &q) const {
|
||||
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
|
||||
if (H_p) {
|
||||
*H_p << skewSymmetric(-q.vector());
|
||||
}
|
||||
if (H_q) {
|
||||
*H_q << skewSymmetric(vector());
|
||||
}
|
||||
|
||||
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
|
||||
x_ * q.y_ - y_ * q.x_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::dot(const Point3 &q) const {
|
||||
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
|
||||
if (H_p) {
|
||||
*H_p << q.vector().transpose();
|
||||
}
|
||||
if (H_q) {
|
||||
*H_q << vector().transpose();
|
||||
}
|
||||
|
||||
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
|
||||
}
|
||||
|
||||
|
|
@ -116,6 +146,12 @@ ostream &operator<<(ostream &os, const Point3& p) {
|
|||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
|
||||
os << p.first << " <-> " << p.second;
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -37,8 +37,8 @@ namespace gtsam {
|
|||
|
||||
private:
|
||||
|
||||
double x_, y_, z_;
|
||||
|
||||
double x_, y_, z_;
|
||||
|
||||
public:
|
||||
enum { dimension = 3 };
|
||||
|
||||
|
|
@ -56,7 +56,7 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// Construct from 3-element vector
|
||||
Point3(const Vector3& v) {
|
||||
explicit Point3(const Vector3& v) {
|
||||
x_ = v(0);
|
||||
y_ = v(1);
|
||||
z_ = v(2);
|
||||
|
|
@ -82,6 +82,11 @@ namespace gtsam {
|
|||
/// inverse
|
||||
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
|
||||
|
||||
/// add vector on right
|
||||
inline Point3 operator +(const Vector3& v) const {
|
||||
return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
|
||||
}
|
||||
|
||||
/// add
|
||||
Point3 operator + (const Point3& q) const;
|
||||
|
||||
|
|
@ -99,20 +104,8 @@ namespace gtsam {
|
|||
Point3 operator / (double s) const;
|
||||
|
||||
/** distance between two points */
|
||||
inline double distance(const Point3& p2,
|
||||
OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
|
||||
double d = (p2 - *this).norm();
|
||||
if (H1) {
|
||||
*H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
|
||||
*H1 = *H1 *(1./d);
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
|
||||
*H2 << *H2 *(1./d);
|
||||
}
|
||||
return d;
|
||||
}
|
||||
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||
|
||||
/** @deprecated The following function has been deprecated, use distance above */
|
||||
inline double dist(const Point3& p2) const {
|
||||
|
|
@ -126,17 +119,19 @@ namespace gtsam {
|
|||
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/** cross product @return this x q */
|
||||
Point3 cross(const Point3 &q) const;
|
||||
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
|
||||
OptionalJacobian<3, 3> H_q = boost::none) const;
|
||||
|
||||
/** dot product @return this * q*/
|
||||
double dot(const Point3 &q) const;
|
||||
double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
|
||||
OptionalJacobian<1, 3> H_q = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// equality
|
||||
bool operator ==(const Point3& q) const;
|
||||
bool operator ==(const Point3& q) const;
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
Vector3 vector() const { return Vector3(x_,y_,z_); }
|
||||
|
|
@ -192,6 +187,10 @@ namespace gtsam {
|
|||
/// @}
|
||||
};
|
||||
|
||||
// Convenience typedef
|
||||
typedef std::pair<Point3, Point3> Point3Pair;
|
||||
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
|
||||
|
||||
/// Syntactic sugar for multiplying coordinates by a scalar s*p
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,10 +32,6 @@
|
|||
# pragma clang diagnostic pop
|
||||
#endif
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
#endif
|
||||
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
|
@ -46,14 +42,13 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
|
||||
Unit3 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix3 D_p_point;
|
||||
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
|
||||
// Calculate the 2*3 Jacobian
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix3 D_p_point;
|
||||
Point3 normalized = point.normalize(H ? &D_p_point : 0);
|
||||
Unit3 direction;
|
||||
direction.p_ = normalized.vector();
|
||||
if (H)
|
||||
*H << direction.basis().transpose() * D_p_point;
|
||||
}
|
||||
return direction;
|
||||
}
|
||||
|
||||
|
|
@ -69,43 +64,97 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
|
|||
return Unit3(d[0], d[1], d[2]);
|
||||
}
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
tbb::mutex unit3BasisMutex;
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Matrix32& Unit3::basis() const {
|
||||
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
|
||||
#ifdef GTSAM_USE_TBB
|
||||
tbb::mutex::scoped_lock lock(unit3BasisMutex);
|
||||
// NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I
|
||||
// can't see the reason why and I can no longer reproduce it. It may have been a red herring, or
|
||||
// there is still a latent bug to watch out for.
|
||||
tbb::mutex::scoped_lock lock(B_mutex_);
|
||||
#endif
|
||||
|
||||
// Return cached version if exists
|
||||
if (B_) return *B_;
|
||||
// Return cached basis if available and the Jacobian isn't needed.
|
||||
if (B_ && !H) {
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Return cached basis and derivatives if available.
|
||||
if (B_ && H && H_B_) {
|
||||
*H = *H_B_;
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Get the unit vector and derivative wrt this.
|
||||
// NOTE(hayk): We can't call point3(), because it would recursively call basis().
|
||||
const Point3 n(p_);
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Vector3 axis;
|
||||
double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
|
||||
if ((mx <= my) && (mx <= mz))
|
||||
axis = Vector3(1.0, 0.0, 0.0);
|
||||
else if ((my <= mx) && (my <= mz))
|
||||
axis = Vector3(0.0, 1.0, 0.0);
|
||||
else if ((mz <= mx) && (mz <= my))
|
||||
axis = Vector3(0.0, 0.0, 1.0);
|
||||
else
|
||||
Point3 axis;
|
||||
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
|
||||
if ((mx <= my) && (mx <= mz)) {
|
||||
axis = Point3(1.0, 0.0, 0.0);
|
||||
} else if ((my <= mx) && (my <= mz)) {
|
||||
axis = Point3(0.0, 1.0, 0.0);
|
||||
} else if ((mz <= mx) && (mz <= my)) {
|
||||
axis = Point3(0.0, 0.0, 1.0);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
// Create the two basis vectors
|
||||
Vector3 b1 = p_.cross(axis).normalized();
|
||||
Vector3 b2 = p_.cross(b1).normalized();
|
||||
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
|
||||
// the chosen axis.
|
||||
Matrix33 H_B1_n;
|
||||
Point3 B1 = n.cross(axis, H ? &H_B1_n : 0);
|
||||
|
||||
// Create the basis matrix
|
||||
// Normalize result to get a unit vector: b1 = B1 / |B1|.
|
||||
Matrix33 H_b1_B1;
|
||||
Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0);
|
||||
|
||||
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
|
||||
// No need to normalize this, p and b1 are orthogonal unit vectors.
|
||||
Matrix33 H_b2_n, H_b2_b1;
|
||||
Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0);
|
||||
|
||||
// Create the basis by stacking b1 and b2.
|
||||
B_.reset(Matrix32());
|
||||
(*B_) << b1, b2;
|
||||
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
|
||||
if (H) {
|
||||
// Chain rule tomfoolery to compute the derivative.
|
||||
const Matrix32& H_n_p = *B_;
|
||||
Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
|
||||
Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
|
||||
|
||||
// Cache the derivative and fill the result.
|
||||
H_B_.reset(Matrix62());
|
||||
(*H_B_) << H_b1_p, H_b2_p;
|
||||
*H = *H_B_;
|
||||
}
|
||||
|
||||
return *B_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// The print fuction
|
||||
Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return Point3(p_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
|
||||
os << pair.p_ << endl;
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Unit3::print(const std::string& s) const {
|
||||
cout << s << ":" << p_ << endl;
|
||||
}
|
||||
|
|
@ -116,11 +165,72 @@ Matrix3 Unit3::skew() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
|
||||
double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
|
||||
// Get the unit vectors of each, and the derivative.
|
||||
Matrix32 H_pn_p;
|
||||
Point3 pn = point3(H_p ? &H_pn_p : 0);
|
||||
|
||||
Matrix32 H_qn_q;
|
||||
Point3 qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// Compute the dot product of the Point3s.
|
||||
Matrix13 H_dot_pn, H_dot_qn;
|
||||
double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0);
|
||||
|
||||
if (H_p) {
|
||||
(*H_p) << H_dot_pn * H_pn_p;
|
||||
}
|
||||
|
||||
if (H_q) {
|
||||
(*H_q) = H_dot_qn * H_qn_q;
|
||||
}
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
|
||||
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
|
||||
Vector2 xi = basis().transpose() * q.p_;
|
||||
if (H)
|
||||
*H = basis().transpose() * q.basis();
|
||||
Matrix23 Bt = basis().transpose();
|
||||
Vector2 xi = Bt * q.p_;
|
||||
if (H_q) {
|
||||
*H_q = Bt * q.basis();
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
|
||||
// Get the point3 of this, and the derivative.
|
||||
Matrix32 H_qn_q;
|
||||
Point3 qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// 2D error here is projecting q into the tangent plane of this (p).
|
||||
Matrix62 H_B_p;
|
||||
Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose();
|
||||
Vector2 xi = Bt * qn.vector();
|
||||
|
||||
if (H_p) {
|
||||
// Derivatives of each basis vector.
|
||||
const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
|
||||
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
|
||||
|
||||
// Derivatives of the two entries of xi wrt the basis vectors.
|
||||
Matrix13 H_xi1_b1 = qn.vector().transpose();
|
||||
Matrix13 H_xi2_b2 = qn.vector().transpose();
|
||||
|
||||
// Assemble dxi/dp = dxi/dB * dB/dp.
|
||||
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
|
||||
Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
|
||||
*H_p << H_xi1_p, H_xi2_p;
|
||||
}
|
||||
|
||||
if (H_q) {
|
||||
// dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
|
||||
Matrix23 H_xi_qu = Bt;
|
||||
*H_q = H_xi_qu * H_qn_q;
|
||||
}
|
||||
|
||||
return xi;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
|
@ -31,6 +32,10 @@
|
|||
|
||||
#include <string>
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Represents a 3D point on a unit sphere.
|
||||
|
|
@ -40,6 +45,11 @@ private:
|
|||
|
||||
Vector3 p_; ///< The location of the point on the unit sphere
|
||||
mutable boost::optional<Matrix32> B_; ///< Cached basis
|
||||
mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis.
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -71,6 +81,12 @@ public:
|
|||
p_.normalize();
|
||||
}
|
||||
|
||||
/// Construct from 2D point in plane at focal length f
|
||||
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
|
||||
explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) {
|
||||
p_.normalize();
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Unit3 FromPoint3(const Point3& point, //
|
||||
OptionalJacobian<2, 3> H = boost::none);
|
||||
|
|
@ -83,6 +99,8 @@ public:
|
|||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
|
||||
|
||||
/// The print fuction
|
||||
void print(const std::string& s = std::string()) const;
|
||||
|
||||
|
|
@ -99,37 +117,50 @@ public:
|
|||
* Returns the local coordinate frame to tangent plane
|
||||
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
|
||||
* tangent to the sphere at the current direction.
|
||||
* Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
|
||||
*/
|
||||
const Matrix32& basis() const;
|
||||
const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
|
||||
|
||||
/// Return skew-symmetric associated with 3D point on unit sphere
|
||||
Matrix3 skew() const;
|
||||
|
||||
/// Return unit-norm Point3
|
||||
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return Point3(p_);
|
||||
}
|
||||
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
|
||||
|
||||
/// Return unit-norm Vector
|
||||
const Vector3& unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
|
||||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Unit3& d) {
|
||||
return Point3(s * d.p_);
|
||||
}
|
||||
|
||||
/// Return dot product with q
|
||||
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
|
||||
OptionalJacobian<1,2> H2 = boost::none) const;
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
|
||||
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
|
||||
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
|
||||
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
|
||||
OptionalJacobian<2, 2> H_q = boost::none) const;
|
||||
|
||||
/// Distance between two directions
|
||||
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
|
||||
|
||||
/// Cross-product between two Unit3s
|
||||
Unit3 cross(const Unit3& q) const {
|
||||
return Unit3(p_.cross(q.p_));
|
||||
}
|
||||
|
||||
/// Cross-product w Point3
|
||||
Point3 cross(const Point3& q) const {
|
||||
return point3().cross(q);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
|
|||
|
|
@ -18,11 +18,17 @@
|
|||
* @brief Tests the Unit3 class
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
|
|
@ -35,6 +41,7 @@
|
|||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
using gtsam::symbol_shorthand::U;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Unit3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
|
||||
|
|
@ -69,7 +76,7 @@ TEST(Unit3, rotate) {
|
|||
Unit3 actual = R * p;
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expectedH = numericalDerivative21(rotate_, R, p);
|
||||
R.rotate(p, actualH, boost::none);
|
||||
|
|
@ -95,6 +102,7 @@ TEST(Unit3, unrotate) {
|
|||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expectedH = numericalDerivative21(unrotate_, R, p);
|
||||
R.unrotate(p, actualH, boost::none);
|
||||
|
|
@ -107,6 +115,37 @@ TEST(Unit3, unrotate) {
|
|||
}
|
||||
}
|
||||
|
||||
TEST(Unit3, dot) {
|
||||
Unit3 p(1, 0.2, 0.3);
|
||||
Unit3 q = p.retract(Vector2(0.5, 0));
|
||||
Unit3 r = p.retract(Vector2(0.8, 0));
|
||||
Unit3 t = p.retract(Vector2(0, 0.3));
|
||||
EXPECT(assert_equal(1.0, p.dot(p), 1e-8));
|
||||
EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
|
||||
EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
|
||||
EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobians
|
||||
Matrix H1, H2;
|
||||
boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
|
||||
boost::none, boost::none);
|
||||
{
|
||||
p.dot(q, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9));
|
||||
}
|
||||
{
|
||||
p.dot(r, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9));
|
||||
}
|
||||
{
|
||||
p.dot(t, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, error) {
|
||||
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
|
||||
|
|
@ -116,6 +155,7 @@ TEST(Unit3, error) {
|
|||
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalDerivative11<Vector2,Unit3>(
|
||||
boost::bind(&Unit3::error, &p, _1, boost::none), q);
|
||||
|
|
@ -130,6 +170,45 @@ TEST(Unit3, error) {
|
|||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, error2) {
|
||||
Unit3 p(0.1, -0.2, 0.8);
|
||||
Unit3 q = p.retract(Vector2(0.2, -0.1));
|
||||
Unit3 r = p.retract(Vector2(0.8, 0));
|
||||
|
||||
// Hard-coded as simple regression values
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8));
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
|
||||
p.errorVector(q, actual, boost::none);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
|
||||
p.errorVector(r, actual, boost::none);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
|
||||
p.errorVector(q, boost::none, actual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
|
||||
p.errorVector(r, boost::none, actual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, distance) {
|
||||
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
|
||||
|
|
@ -155,6 +234,11 @@ TEST(Unit3, distance) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates0) {
|
||||
Unit3 p;
|
||||
Vector actual = p.localCoordinates(p);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
|
||||
TEST(Unit3, localCoordinates) {
|
||||
{
|
||||
|
|
@ -219,12 +303,45 @@ TEST(Unit3, localCoordinates) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Wrapper to make basis return a vector6 so we can test numerical derivatives.
|
||||
Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
|
||||
Matrix32 B = p.basis(H);
|
||||
Vector6 B_vec;
|
||||
B_vec << B.col(0), B.col(1);
|
||||
return B_vec;
|
||||
}
|
||||
|
||||
TEST(Unit3, basis) {
|
||||
Unit3 p;
|
||||
Matrix32 expected;
|
||||
expected << 0, 0, 0, -1, 1, 0;
|
||||
Matrix actual = p.basis();
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Unit3 p(0.1, -0.2, 0.9);
|
||||
|
||||
Matrix expected(3, 2);
|
||||
expected << 0.0, -0.994169047, 0.97618706,
|
||||
-0.0233922129, 0.216930458, 0.105264958;
|
||||
|
||||
Matrix62 actualH;
|
||||
Matrix actual = p.basis(actualH);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
|
||||
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
/// Check the basis derivatives of a bunch of random Unit3s.
|
||||
TEST(Unit3, basis_derivatives) {
|
||||
int num_tests = 100;
|
||||
boost::mt19937 rng(42);
|
||||
for (int i = 0; i < num_tests; i++) {
|
||||
Unit3 p = Unit3::Random(rng);
|
||||
|
||||
Matrix62 actualH;
|
||||
p.basis(actualH);
|
||||
|
||||
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
|
|
@ -298,6 +415,46 @@ TEST (Unit3, FromPoint3) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, ErrorBetweenFactor) {
|
||||
std::vector<Unit3> data;
|
||||
data.push_back(Unit3(1.0, 0.0, 0.0));
|
||||
data.push_back(Unit3(0.0, 0.0, 1.0));
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
Values initial_values;
|
||||
|
||||
// Add prior factors.
|
||||
SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
|
||||
for (size_t i = 0; i < data.size(); i++) {
|
||||
graph.add(PriorFactor<Unit3>(U(i), data[i], R_prior));
|
||||
}
|
||||
|
||||
// Add process factors using the dot product error function.
|
||||
SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
|
||||
for (size_t i = 0; i < data.size() - 1; i++) {
|
||||
Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
|
||||
graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
|
||||
}
|
||||
|
||||
// Add initial values. Since there is no identity, just pick something.
|
||||
for (size_t i = 0; i < data.size(); i++) {
|
||||
initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0));
|
||||
}
|
||||
|
||||
Values values = GaussNewtonOptimizer(graph, initial_values).optimize();
|
||||
|
||||
// Check that the y-value is very small for each.
|
||||
for (size_t i = 0; i < data.size(); i++) {
|
||||
EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3));
|
||||
}
|
||||
|
||||
// Check that the dot product between variables is close to 1.
|
||||
for (size_t i = 0; i < data.size() - 1; i++) {
|
||||
EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(actualH, Serialization) {
|
||||
Unit3 p(0, 1, 0);
|
||||
|
|
|
|||
|
|
@ -163,8 +163,6 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim,
|
|||
return assert_equal(Matrix(Q), actual, 1e-4);
|
||||
}
|
||||
|
||||
#if 1
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, StraightLine) {
|
||||
// Set up IMU measurements
|
||||
|
|
@ -610,7 +608,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
EXPECT(
|
||||
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega()));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) {
|
||||
|
|
@ -679,7 +676,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
hist(samples(:,i), 500)
|
||||
end
|
||||
*/
|
||||
size_t N = 10000;
|
||||
size_t N = 100000;
|
||||
Matrix samples(9,N);
|
||||
Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284);
|
||||
Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10);
|
||||
|
|
@ -698,7 +695,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
#endif
|
||||
|
||||
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor,
|
||||
measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 10000));
|
||||
measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
|
||||
|
||||
// Matrix expected(9,9);
|
||||
// expected <<
|
||||
|
|
@ -837,8 +834,8 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
Pose3 x1;
|
||||
Vector3 v1 = Vector3(0, 0, 0);
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none,
|
||||
measuredAcc, measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar)));
|
||||
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
|
||||
Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
|
@ -23,6 +23,7 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <numeric>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
@ -74,7 +75,7 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Error function *without* the NoiseModel, \f$ h(x)-z \f$.
|
||||
* Error function *without* the NoiseModel, \f$ z-h(x) -> Local(h(x),z) \f$.
|
||||
* We override this method to provide
|
||||
* both the function evaluation and its derivative(s) in H.
|
||||
*/
|
||||
|
|
@ -82,10 +83,12 @@ public:
|
|||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
if (H) {
|
||||
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
|
||||
return traits<T>::Local(measured_, value);
|
||||
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
|
||||
// because it would use the tangent space of the measurement instead of the value.
|
||||
return -traits<T>::Local(value, measured_);
|
||||
} else {
|
||||
const T value = expression_.value(x);
|
||||
return traits<T>::Local(measured_, value);
|
||||
return -traits<T>::Local(value, measured_);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -96,7 +99,7 @@ public:
|
|||
|
||||
// In case noise model is constrained, we need to provide a noise model
|
||||
SharedDiagonal noiseModel;
|
||||
if (noiseModel_->isConstrained()) {
|
||||
if (noiseModel_ && noiseModel_->isConstrained()) {
|
||||
noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
|
||||
noiseModel_)->unit();
|
||||
}
|
||||
|
|
@ -116,11 +119,13 @@ public:
|
|||
T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
|
||||
|
||||
// Evaluate error and set RHS vector b
|
||||
Ab(size()).col(0) = -traits<T>::Local(measured_, value);
|
||||
Ab(size()).col(0) = traits<T>::Local(value, measured_);
|
||||
|
||||
// Whiten the corresponding system, Ab already contains RHS
|
||||
Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
|
||||
noiseModel_->WhitenSystem(Ab.matrix(), b);
|
||||
if (noiseModel_) {
|
||||
Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
|
||||
noiseModel_->WhitenSystem(Ab.matrix(), b);
|
||||
}
|
||||
|
||||
return factor;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -81,23 +81,33 @@ TEST(BearingFactor, Serialization3D) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingFactor, 3D) {
|
||||
// Serialize the factor
|
||||
std::string serialized = serializeXML(factor3D);
|
||||
|
||||
// And de-serialize it
|
||||
BearingFactor3D factor;
|
||||
deserializeXML(serialized, factor);
|
||||
|
||||
// Set the linearization point
|
||||
Values values;
|
||||
values.insert(poseKey, Pose3());
|
||||
values.insert(pointKey, Point3(1, 0, 0));
|
||||
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
|
||||
values, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
}
|
||||
// TODO(frank): this test is disabled (for now) because the macros below are
|
||||
// incompatible with the Unit3 localCoordinates. The issue is the following:
|
||||
// For factors, we want to use Local(value, measured), because we need the error
|
||||
// to be expressed in the tangent space of value. This surfaced in the Unit3 case
|
||||
// where the tangent space can be radically didfferent from one value to the next.
|
||||
// For derivatives, we want Local(constant, varying), because we need a derivative
|
||||
// in a constant tangent space. But since the macros below call whitenedError
|
||||
// which calls Local(value,measured), we actually call the reverse. This does not
|
||||
// matter for types with a commutative Local, but matters a lot for Unit3.
|
||||
// More thinking needed about what the right thing is, here...
|
||||
//TEST(BearingFactor, 3D) {
|
||||
// // Serialize the factor
|
||||
// std::string serialized = serializeXML(factor3D);
|
||||
//
|
||||
// // And de-serialize it
|
||||
// BearingFactor3D factor;
|
||||
// deserializeXML(serialized, factor);
|
||||
//
|
||||
// // Set the linearization point
|
||||
// Values values;
|
||||
// values.insert(poseKey, Pose3());
|
||||
// values.insert(pointKey, Point3(1, 0, 0));
|
||||
//
|
||||
// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
|
||||
// values, 1e-7, 1e-5);
|
||||
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -80,23 +80,25 @@ TEST(BearingRangeFactor, Serialization3D) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, 3D) {
|
||||
// Serialize the factor
|
||||
std::string serialized = serializeXML(factor3D);
|
||||
|
||||
// And de-serialize it
|
||||
BearingRangeFactor3D factor;
|
||||
deserializeXML(serialized, factor);
|
||||
|
||||
// Set the linearization point
|
||||
Values values;
|
||||
values.insert(poseKey, Pose3());
|
||||
values.insert(pointKey, Point3(1, 0, 0));
|
||||
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
|
||||
values, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
}
|
||||
// TODO(frank): this test is disabled (for now) because the macros below are
|
||||
// incompatible with the Unit3 localCoordinates. See testBearingFactor...
|
||||
//TEST(BearingRangeFactor, 3D) {
|
||||
// // Serialize the factor
|
||||
// std::string serialized = serializeXML(factor3D);
|
||||
//
|
||||
// // And de-serialize it
|
||||
// BearingRangeFactor3D factor;
|
||||
// deserializeXML(serialized, factor);
|
||||
//
|
||||
// // Set the linearization point
|
||||
// Values values;
|
||||
// values.insert(poseKey, Pose3());
|
||||
// values.insert(pointKey, Point3(1, 0, 0));
|
||||
//
|
||||
// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
|
||||
// values, 1e-7, 1e-5);
|
||||
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -85,11 +85,11 @@ namespace gtsam {
|
|||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) (*H) = eye(traits<T>::GetDimension(p));
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) (*H) = eye(traits<T>::GetDimension(x));
|
||||
// manifold equivalent of z-x -> Local(x,z)
|
||||
// TODO(ASL) Add Jacobians.
|
||||
return traits<T>::Local(prior_,p);
|
||||
return -traits<T>::Local(x, prior_);
|
||||
}
|
||||
|
||||
const VALUE & prior() const { return prior_; }
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ int main(int argc, char** argv){
|
|||
string calibration_loc = findExampleDataFile("VO_calibration.txt");
|
||||
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
|
||||
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
|
||||
|
||||
|
||||
//read camera calibration info from file
|
||||
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
|
||||
cout << "Reading calibration info" << endl;
|
||||
|
|
@ -67,17 +67,17 @@ int main(int argc, char** argv){
|
|||
cout << "Reading camera poses" << endl;
|
||||
ifstream pose_file(pose_loc.c_str());
|
||||
|
||||
int i;
|
||||
int pose_index;
|
||||
MatrixRowMajor m(4,4);
|
||||
//read camera pose parameters and use to make initial estimates of camera poses
|
||||
while (pose_file >> i) {
|
||||
while (pose_file >> pose_index) {
|
||||
for (int i = 0; i < 16; i++)
|
||||
pose_file >> m.data()[i];
|
||||
initial_estimate.insert(i, Pose3(m));
|
||||
initial_estimate.insert(pose_index, Pose3(m));
|
||||
}
|
||||
|
||||
|
||||
// landmark keys
|
||||
size_t l;
|
||||
size_t landmark_key;
|
||||
|
||||
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
|
||||
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
|
||||
|
|
@ -90,14 +90,14 @@ int main(int argc, char** argv){
|
|||
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
|
||||
size_t current_l = 3; // hardcoded landmark ID from first measurement
|
||||
|
||||
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
|
||||
while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) {
|
||||
|
||||
if(current_l != l) {
|
||||
if(current_l != landmark_key) {
|
||||
graph.push_back(factor);
|
||||
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
|
||||
current_l = l;
|
||||
current_l = landmark_key;
|
||||
}
|
||||
factor->add(Point2(uL,v), i);
|
||||
factor->add(Point2(uL,v), pose_index);
|
||||
}
|
||||
|
||||
Pose3 firstPose = initial_estimate.at<Pose3>(1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue