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

Merge remote-tracking branch 'origin/develop' into fan/prototype-hybrid-tr

release/4.3a0
Fan Jiang 2022-05-08 11:45:22 -07:00
parent 2de3169976 63ffeafd0d
commit 1bf995234a
36 changed files with 1317 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/base/OptionalJacobian.h
View File

@ -20,6 +20,8 @@
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#include <stdexcept>
#include <string>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
@ -96,6 +98,24 @@ public:
usurp(dynamic->data());
}
/**
* @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
* @note This is important so we don't overwrite someone else's memory!
*/
template<class MATRIX>
OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
map_(nullptr) {
if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
usurp(dynamic_ref.data());
} else {
throw std::invalid_argument(
std::string("OptionalJacobian called with wrong dimensions or "
"storage order.\n"
"Expected: ") +
"(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
}
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty

12
gtsam/geometry/Point2.cpp
View File

@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
return circleCircleIntersection(c1, c2, fh);
}
Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
const size_t n = abPointPairs.size();
if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
Point2 aSum(0, 0), bSum(0, 0);
for (const Point2Pair &abPair : abPointPairs) {
aSum += abPair.first;
bSum += abPair.second;
}
const double f = 1.0 / n;
return {aSum * f, bSum * f};
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
os << p.first << " <-> " << p.second;

3
gtsam/geometry/Point2.h
View File

@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
/// Calculate the two means of a set of Point2 pairs
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
/**
* @brief Intersect 2 circles

2
gtsam/geometry/Pose2.cpp
View File

@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
"Pose2:Align expects 2*N matrices of equal shape.");
}
Point2Pairs ab_pairs;
for (size_t j=0; j < a.cols(); j++) {
for (Eigen::Index j = 0; j < a.cols(); j++) {
ab_pairs.emplace_back(a.col(j), b.col(j));
}
return Pose2::Align(ab_pairs);

4
gtsam/geometry/Pose2.h
View File

@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};

2
gtsam/geometry/Pose3.cpp
View File

@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
"Pose3:Align expects 3*N matrices of equal shape.");
}
Point3Pairs abPointPairs;
for (size_t j=0; j < a.cols(); j++) {
for (Eigen::Index j = 0; j < a.cols(); j++) {
abPointPairs.emplace_back(a.col(j), b.col(j));
}
return Pose3::Align(abPointPairs);

13
gtsam/geometry/Rot2.cpp
View File

@ -129,6 +129,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
}
}
/* ************************************************************************* */
Rot2 Rot2::ClosestTo(const Matrix2& M) {
Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Matrix2& U = svd.matrixU();
const Matrix2& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();
Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
double c = M_prime(0, 0);
double s = M_prime(1, 0);
return Rot2::fromCosSin(c, s);
}
/* ************************************************************************* */
} // gtsam

4
gtsam/geometry/Rot2.h
View File

@ -14,6 +14,7 @@
* @brief 2D rotation
* @date Dec 9, 2009
* @author Frank Dellaert
* @author John Lambert
*/
#pragma once
@ -209,6 +210,9 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */
Matrix2 transpose() const;
/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);
private:
/** Serialization function */
friend class boost::serialization::access;

242
gtsam/geometry/Similarity2.cpp Normal file
View File

@ -0,0 +1,242 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Similarity2.cpp
* @brief Implementation of Similarity2 transform
* @author John Lambert, Varun Agrawal
*/
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
namespace gtsam {
using std::vector;
namespace internal {
/// Subtract centroids from point pairs.
static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
const Point2Pair& centroids) {
Point2Pairs d_abPointPairs;
for (const Point2Pair& abPair : abPointPairs) {
Point2 da = abPair.first - centroids.first;
Point2 db = abPair.second - centroids.second;
d_abPointPairs.emplace_back(da, db);
}
return d_abPointPairs;
}
/// Form inner products x and y and calculate scale.
static double CalculateScale(const Point2Pairs& d_abPointPairs,
const Rot2& aRb) {
double x = 0, y = 0;
Point2 da, db;
for (const Point2Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
const Vector2 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
}
const double s = y / x;
return s;
}
/// Form outer product H.
static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) {
Matrix2 H = Z_2x2;
for (const Point2Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
}
return H;
}
/**
* @brief This method estimates the similarity transform from differences point
* pairs, given a known or estimated rotation and point centroids.
*
* @param d_abPointPairs
* @param aRb
* @param centroids
* @return Similarity2
*/
static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
const Point2Pair& centroids) {
const double s = CalculateScale(d_abPointPairs, aRb);
// dividing aTb by s is required because the registration cost function
// minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity2(aRb, aTb, s);
}
/**
* @brief This method estimates the similarity transform from point pairs,
* given a known or estimated rotation.
* Refer to:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
* Chapter 3
*
* @param abPointPairs
* @param aRb
* @return Similarity2
*/
static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs,
const Rot2& aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
return internal::Align(d_abPointPairs, aRb, centroids);
}
} // namespace internal
Similarity2::Similarity2() : t_(0, 0), s_(1) {}
Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
Similarity2::Similarity2(const Rot2& R, const Point2& t, double s)
: R_(R), t_(t), s_(s) {}
Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s)
: R_(Rot2::ClosestTo(R)), t_(t), s_(s) {}
Similarity2::Similarity2(const Matrix3& T)
: R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())),
t_(T.topRightCorner<2, 1>()),
s_(1.0 / T(2, 2)) {}
bool Similarity2::equals(const Similarity2& other, double tol) const {
return R_.equals(other.R_, tol) &&
traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
s_ > (other.s_ - tol);
}
bool Similarity2::operator==(const Similarity2& other) const {
return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
}
void Similarity2::print(const std::string& s) const {
std::cout << std::endl;
std::cout << s;
rotation().print("\nR:\n");
std::cout << "t: " << translation().transpose() << " s: " << scale()
<< std::endl;
}
Similarity2 Similarity2::identity() { return Similarity2(); }
Similarity2 Similarity2::operator*(const Similarity2& S) const {
return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
}
Similarity2 Similarity2::inverse() const {
const Rot2 Rt = R_.inverse();
const Point2 sRt = Rt * (-s_ * t_);
return Similarity2(Rt, sRt, 1.0 / s_);
}
Point2 Similarity2::transformFrom(const Point2& p) const {
const Point2 q = R_ * p + t_;
return s_ * q;
}
Pose2 Similarity2::transformFrom(const Pose2& T) const {
Rot2 R = R_.compose(T.rotation());
Point2 t = Point2(s_ * (R_ * T.translation() + t_));
return Pose2(R, t);
}
Point2 Similarity2::operator*(const Point2& p) const {
return transformFrom(p);
}
Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) {
// Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 2)
throw std::runtime_error("input should have at least 2 pairs of points");
auto centroids = means(abPointPairs);
auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
Matrix2 H = internal::CalculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot2 aRb = Rot2::ClosestTo(H);
return internal::Align(d_abPointPairs, aRb, centroids);
}
Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
const size_t n = abPosePairs.size();
if (n < 2)
throw std::runtime_error("input should have at least 2 pairs of poses");
// calculate rotation
vector<Rot2> rotations;
Point2Pairs abPointPairs;
rotations.reserve(n);
abPointPairs.reserve(n);
// Below denotes the pose of the i'th object/camera/etc
// in frame "a" or frame "b".
Pose2 aTi, bTi;
for (const Pose2Pair& abPair : abPosePairs) {
std::tie(aTi, bTi) = abPair;
const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation());
}
const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
return internal::AlignGivenR(abPointPairs, aRb_estimate);
}
Vector4 Similarity2::Logmap(const Similarity2& S, //
OptionalJacobian<4, 4> Hm) {
const Vector2 u = S.t_;
const Vector1 w = Rot2::Logmap(S.R_);
const double s = log(S.s_);
Vector4 result;
result << u, w, s;
if (Hm) {
throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
}
return result;
}
Similarity2 Similarity2::Expmap(const Vector4& v, //
OptionalJacobian<4, 4> Hm) {
const Vector2 t = v.head<2>();
const Rot2 R = Rot2::Expmap(v.segment<1>(2));
const double s = v[3];
if (Hm) {
throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
}
return Similarity2(R, t, s);
}
Matrix4 Similarity2::AdjointMap() const {
throw std::runtime_error("Similarity2::AdjointMap not implemented");
}
std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
<< p.scale() << "]\';";
return os;
}
Matrix3 Similarity2::matrix() const {
Matrix3 T;
T.topRows<2>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 1.0 / s_;
return T;
}
} // namespace gtsam

200
gtsam/geometry/Similarity2.h Normal file
View File

@ -0,0 +1,200 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Similarity2.h
* @brief Implementation of Similarity2 transform
* @author John Lambert, Varun Agrawal
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
namespace gtsam {
// Forward declarations
class Pose2;
/**
* 2D similarity transform
*/
class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// @name Pose Concept
/// @{
typedef Rot2 Rotation;
typedef Point2 Translation;
/// @}
private:
Rot2 R_;
Point2 t_;
double s_;
public:
/// @name Constructors
/// @{
/// Default constructor
Similarity2();
/// Construct pure scaling
Similarity2(double s);
/// Construct from GTSAM types
Similarity2(const Rot2& R, const Point2& t, double s);
/// Construct from Eigen types
Similarity2(const Matrix2& R, const Vector2& t, double s);
/// Construct from matrix [R t; 0 s^-1]
Similarity2(const Matrix3& T);
/// @}
/// @name Testable
/// @{
/// Compare with tolerance
bool equals(const Similarity2& sim, double tol) const;
/// Exact equality
bool operator==(const Similarity2& other) const;
/// Print with optional string
void print(const std::string& s) const;
friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
/// @}
/// @name Group
/// @{
/// Return an identity transform
static Similarity2 identity();
/// Composition
Similarity2 operator*(const Similarity2& S) const;
/// Return the inverse
Similarity2 inverse() const;
/// @}
/// @name Group action on Point2
/// @{
/// Action on a point p is s*(R*p+t)
Point2 transformFrom(const Point2& p) const;
/**
* Action on a pose T.
* |Rs ts| |R t| |Rs*R Rs*t+ts|
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object.
* To retrieve a Pose2, we normalized the scale value into 1.
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
* | 0 1/s | = | 0 1 |
*
* This group action satisfies the compatibility condition.
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
*/
Pose2 transformFrom(const Pose2& T) const;
/* syntactic sugar for transformFrom */
Point2 operator*(const Point2& p) const;
/**
* Create Similarity2 by aligning at least two point pairs
*/
static Similarity2 Align(const Point2Pairs& abPointPairs);
/**
* Create the Similarity2 object that aligns at least two pose pairs.
* Each pair is of the form (aTi, bTi).
* Given a list of pairs in frame a, and a list of pairs in frame b,
Align()
* will compute the best-fit Similarity2 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean)
of
* many estimates aRb (from each pair). Afterwards, the scale factor will
be computed
* using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
/// @}
/// @name Lie Group
/// @{
/**
* Log map at the identity
* \f$ [t_x, t_y, \delta, \lambda] \f$
*/
static Vector4 Logmap(const Similarity2& S, //
OptionalJacobian<4, 4> Hm = boost::none);
/// Exponential map at the identity
static Similarity2 Expmap(const Vector4& v, //
OptionalJacobian<4, 4> Hm = boost::none);
/// Chart at the origin
struct ChartAtOrigin {
static Similarity2 Retract(const Vector4& v,
ChartJacobian H = boost::none) {
return Similarity2::Expmap(v, H);
}
static Vector4 Local(const Similarity2& other,
ChartJacobian H = boost::none) {
return Similarity2::Logmap(other, H);
}
};
/// Project from one tangent space to another
Matrix4 AdjointMap() const;
using LieGroup<Similarity2, 4>::inverse;
/// @}
/// @name Standard interface
/// @{
/// Calculate 4*4 matrix group equivalent
Matrix3 matrix() const;
/// Return a GTSAM rotation
Rot2 rotation() const { return R_; }
/// Return a GTSAM translation
Point2 translation() const { return t_; }
/// Return the scale
double scale() const { return s_; }
/// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
inline static size_t Dim() { return 4; }
/// Dimensionality of tangent space = 4 DOF
inline size_t dim() const { return 4; }
/// @}
};
template <>
struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
template <>
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
} // namespace gtsam

20
gtsam/geometry/Similarity3.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
using std::vector;
namespace {
namespace internal {
/// Subtract centroids from point pairs.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) {
@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids);
}
} // namespace
} // namespace internal
Similarity3::Similarity3() :
t_(0,0,0), s_(1) {
@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
if (abPointPairs.size() < 3)
throw std::runtime_error("input should have at least 3 pairs of points");
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs);
auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
Matrix3 H = internal::calculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids);
return internal::align(d_abPointPairs, aRb, centroids);
}
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
}
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb_estimate);
return internal::alignGivenR(abPointPairs, aRb_estimate);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {
@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
return os;
}
const Matrix4 Similarity3::matrix() const {
Matrix4 Similarity3::matrix() const {
Matrix4 T;
T.topRows<3>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
return T;
}
Similarity3::operator Pose3() const {
return Pose3(R_, s_ * t_);
}
} // namespace gtsam

121
gtsam/geometry/Similarity3.h
View File

@ -18,13 +18,12 @@
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
@ -34,108 +33,106 @@ class Pose3;
/**
* 3D similarity transform
*/
class Similarity3: public LieGroup<Similarity3, 7> {
class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// @name Pose Concept
/// @{
typedef Rot3 Rotation;
typedef Point3 Translation;
/// @}
private:
private:
Rot3 R_;
Point3 t_;
double s_;
public:
public:
/// @name Constructors
/// @{
/// Default constructor
GTSAM_EXPORT Similarity3();
Similarity3();
/// Construct pure scaling
GTSAM_EXPORT Similarity3(double s);
Similarity3(double s);
/// Construct from GTSAM types
GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
Similarity3(const Rot3& R, const Point3& t, double s);
/// Construct from Eigen types
GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
Similarity3(const Matrix3& R, const Vector3& t, double s);
/// Construct from matrix [R t; 0 s^-1]
GTSAM_EXPORT Similarity3(const Matrix4& T);
Similarity3(const Matrix4& T);
/// @}
/// @name Testable
/// @{
/// Compare with tolerance
GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
bool equals(const Similarity3& sim, double tol) const;
/// Exact equality
GTSAM_EXPORT bool operator==(const Similarity3& other) const;
bool operator==(const Similarity3& other) const;
/// Print with optional string
GTSAM_EXPORT void print(const std::string& s) const;
void print(const std::string& s) const;
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
/// @}
/// @name Group
/// @{
/// Return an identity transform
GTSAM_EXPORT static Similarity3 identity();
static Similarity3 identity();
/// Composition
GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
Similarity3 operator*(const Similarity3& S) const;
/// Return the inverse
GTSAM_EXPORT Similarity3 inverse() const;
Similarity3 inverse() const;
/// @}
/// @name Group action on Point3
/// @{
/// Action on a point p is s*(R*p+t)
GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
/**
/**
* Action on a pose T.
* |Rs ts| |R t| |Rs*R Rs*t+ts|
* |Rs ts| |R t| |Rs*R Rs*t+ts|
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object.
* To retrieve a Pose3, we normalized the scale value into 1.
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
* | 0 1/s | = | 0 1 |
*
* This group action satisfies the compatibility condition.
*
* This group action satisfies the compatibility condition.
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
*/
GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
Pose3 transformFrom(const Pose3& T) const;
/** syntactic sugar for transformFrom */
GTSAM_EXPORT Point3 operator*(const Point3& p) const;
Point3 operator*(const Point3& p) const;
/**
* Create Similarity3 by aligning at least three point pairs
*/
GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/**
* Create the Similarity3 object that aligns at least two pose pairs.
* Each pair is of the form (aTi, bTi).
* Given a list of pairs in frame a, and a list of pairs in frame b, Align()
* will compute the best-fit Similarity3 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean) of
* many estimates aRb (from each pair). Afterwards, the scale factor will be computed
* using the algorithm described here:
* many estimates aRb (from each pair). Afterwards, the scale factor will be
* computed using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
/// @}
/// @name Lie Group
@ -144,20 +141,22 @@ public:
/** Log map at the identity
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
*/
GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
/** Exponential map at the identity
*/
GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
/// Chart at the origin
struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
static Similarity3 Retract(const Vector7& v,
ChartJacobian H = boost::none) {
return Similarity3::Expmap(v, H);
}
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
static Vector7 Local(const Similarity3& other,
ChartJacobian H = boost::none) {
return Similarity3::Logmap(other, H);
}
};
@ -170,67 +169,53 @@ public:
* @return 4*4 element of Lie algebra that can be exponentiated
* TODO(frank): rename to Hat, make part of traits
*/
GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
static Matrix4 wedge(const Vector7& xi);
/// Project from one tangent space to another
GTSAM_EXPORT Matrix7 AdjointMap() const;
Matrix7 AdjointMap() const;
/// @}
/// @name Standard interface
/// @{
/// Calculate 4*4 matrix group equivalent
GTSAM_EXPORT const Matrix4 matrix() const;
Matrix4 matrix() const;
/// Return a GTSAM rotation
const Rot3& rotation() const {
return R_;
}
Rot3 rotation() const { return R_; }
/// Return a GTSAM translation
const Point3& translation() const {
return t_;
}
Point3 translation() const { return t_; }
/// Return the scale
double scale() const {
return s_;
}
/// Convert to a rigid body pose (R, s*t)
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
GTSAM_EXPORT operator Pose3() const;
double scale() const { return s_; }
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() {
return 7;
}
inline static size_t Dim() { return 7; }
/// Dimensionality of tangent space = 7 DOF
inline size_t dim() const {
return 7;
}
inline size_t dim() const { return 7; }
/// @}
/// @name Helper functions
/// @{
private:
private:
/// Calculate expmap and logmap coefficients.
static Matrix3 GetV(Vector3 w, double lambda);
/// @}
};
template<>
template <>
inline Matrix wedge<Similarity3>(const Vector& xi) {
return Similarity3::wedge(xi);
}
template<>
template <>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
template<>
template <>
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
} // namespace gtsam
} // namespace gtsam

187
gtsam/geometry/geometry.i
View File

@ -547,6 +547,12 @@ class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Constructors from Pose3
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
Eigen::Ref<Eigen::MatrixXd> H);
// Testable
void print(string s = "") const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@ -584,7 +590,13 @@ class Cal3_S2 {
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
@ -623,7 +635,13 @@ virtual class Cal3DS2_Base {
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// enabling serialization functionality
void serialize() const;
@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Note: the signature of this functions differ from the functions
// with equal name in the base class.
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// enabling serialization functionality
void serialize() const;
@ -706,7 +730,13 @@ class Cal3Fisheye {
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
@ -769,7 +799,13 @@ class Cal3Bundler {
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p,
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
@ -807,12 +843,25 @@ class CalibratedCamera {
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
// Standard Interface
gtsam::Pose3 pose() const;
double range(const gtsam::Point3& point) const;
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose) const;
double range(const gtsam::Pose3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
double range(const gtsam::CalibratedCamera& camera) const;
// enabling serialization functionality
@ -824,6 +873,7 @@ template <CALIBRATION>
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
PinholeCamera(const gtsam::Pose3& pose);
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@ -850,14 +900,123 @@ class PinholeCamera {
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
// enabling serialization functionality
void serialize() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
#include <gtsam/geometry/PinholePose.h>
template <CALIBRATION>
class PinholePose {
// Standard Constructors and Named Constructors
PinholePose();
PinholePose(const gtsam::PinholePose<CALIBRATION> other);
PinholePose(const gtsam::Pose3& pose);
PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
static This Level(const gtsam::Pose2& pose, double height);
static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const CALIBRATION* K);
// Testable
void print(string s = "PinholePose") const;
bool equals(const This& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
CALIBRATION calibration() const;
// Manifold
This retract(Vector d) const;
Vector localCoordinates(const This& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
Eigen::Ref<Eigen::MatrixXd> Dpose);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
#include <gtsam/geometry/Similarity2.h>
class Similarity2 {
// Standard Constructors
Similarity2();
Similarity2(double s);
Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s);
Similarity2(const Matrix& R, const Vector& t, double s);
Similarity2(const Matrix& T);
gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
gtsam::Pose2 transformFrom(const gtsam::Pose2& T);
static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs);
static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
// Standard Interface
bool equals(const gtsam::Similarity2& sim, double tol) const;
Matrix matrix() const;
gtsam::Rot2& rotation();
gtsam::Point2& translation();
double scale() const;
};
#include <gtsam/geometry/Similarity3.h>
class Similarity3 {
// Standard Constructors
@ -874,22 +1033,13 @@ class Similarity3 {
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
// Standard Interface
const Matrix matrix() const;
const gtsam::Rot3& rotation();
const gtsam::Point3& translation();
bool equals(const gtsam::Similarity3& sim, double tol) const;
Matrix matrix() const;
gtsam::Rot3& rotation();
gtsam::Point3& translation();
double scale() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
template <T>
class CameraSet {
CameraSet();
@ -921,9 +1071,18 @@ class StereoCamera {
static size_t Dim();
// Transformations and measurement functions
gtsam::StereoPoint2 project(const gtsam::Point3& point);
gtsam::StereoPoint2 project(const gtsam::Point3& point) const;
gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
// project with Jacobian
gtsam::StereoPoint2 project2(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Point3 backproject2(const gtsam::StereoPoint2& p,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
// enabling serialization functionality
void serialize() const;
};

66
gtsam/geometry/tests/testSimilarity2.cpp Normal file
View File

@ -0,0 +1,66 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testSimilarity2.cpp
* @brief Unit tests for Similarity2 class
* @author Varun Agrawal
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Similarity2.h>
#include <functional>
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(Similarity2)
static const Point2 P(0.2, 0.7);
static const Rot2 R = Rot2::fromAngle(0.3);
static const double s = 4;
const double degree = M_PI / 180;
//******************************************************************************
TEST(Similarity2, Concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>));
BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
}
//******************************************************************************
TEST(Similarity2, Constructors) {
Similarity2 sim2_Construct1;
Similarity2 sim2_Construct2(s);
Similarity2 sim2_Construct3(R, P, s);
Similarity2 sim2_Construct4(R.matrix(), P, s);
}
//******************************************************************************
TEST(Similarity2, Getters) {
Similarity2 sim2_default;
EXPECT(assert_equal(Rot2(), sim2_default.rotation()));
EXPECT(assert_equal(Point2(0, 0), sim2_default.translation()));
EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9);
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

22
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
Values result;
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
//result.print("Optimized Estimate\n");
Pose3 p1, p2, p3, p4, p5;
p1 = Pose3(result.at<Similarity3>(X(1)));
p2 = Pose3(result.at<Similarity3>(X(2)));
p3 = Pose3(result.at<Similarity3>(X(3)));
p4 = Pose3(result.at<Similarity3>(X(4)));
p5 = Pose3(result.at<Similarity3>(X(5)));
Similarity3 p1, p2, p3, p4, p5;
p1 = result.at<Similarity3>(X(1));
p2 = result.at<Similarity3>(X(2));
p3 = result.at<Similarity3>(X(3));
p4 = result.at<Similarity3>(X(4));
p5 = result.at<Similarity3>(X(5));
//p1.print("Pose1");
//p2.print("Pose2");
//p3.print("Pose3");
//p4.print("Pose4");
//p5.print("Pose5");
//p1.print("Similarity1");
//p2.print("Similarity2");
//p3.print("Similarity3");
//p4.print("Similarity4");
//p5.print("Similarity5");
Similarity3 expected(0.7);
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));

151
gtsam/linear/LossFunctions.h
View File

@ -54,23 +54,31 @@ namespace noiseModel {
// clang-format on
namespace mEstimator {
//---------------------------------------------------------------------------------------
/**
* Pure virtual class for all robust error function classes.
*
* It provides the machinery for block vs scalar reweighting strategies, in
* addition to defining the interface of derived classes.
*/
class GTSAM_EXPORT Base {
public:
/** the rows can be weighted independently according to the error
* or uniformly with the norm of the right hand side */
enum ReweightScheme { Scalar, Block };
typedef boost::shared_ptr<Base> shared_ptr;
protected:
/** the rows can be weighted independently according to the error
* or uniformly with the norm of the right hand side */
/// Strategy for reweighting \sa ReweightScheme
ReweightScheme reweight_;
public:
Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
virtual ~Base() {}
/*
/// Returns the reweight scheme, as explained in ReweightScheme
ReweightScheme reweightScheme() const { return reweight_; }
/**
* This method is responsible for returning the total penalty for a given
* amount of error. For example, this method is responsible for implementing
* the quadratic function for an L2 penalty, the absolute value function for
@ -80,16 +88,20 @@ class GTSAM_EXPORT Base {
* error vector, then it prevents implementations of asymmeric loss
* functions. It would be better for this function to accept the vector and
* internally call the norm if necessary.
*
* This returns \rho(x) in \ref mEstimator
*/
virtual double loss(double distance) const { return 0; };
virtual double loss(double distance) const { return 0; }
/*
/**
* This method is responsible for returning the weight function for a given
* amount of error. The weight function is related to the analytic derivative
* of the loss function. See
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* for details. This method is required when optimizing cost functions with
* robust penalties using iteratively re-weighted least squares.
*
* This returns w(x) in \ref mEstimator
*/
virtual double weight(double distance) const = 0;
@ -124,7 +136,15 @@ class GTSAM_EXPORT Base {
}
};
/// Null class should behave as Gaussian
/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or
* plain least-squares (non-robust).
*
* This model has no additional parameters.
*
* - Loss \rho(x) = 0.5 x²
* - Derivative \phi(x) = x
* - Weight w(x) = \phi(x)/x = 1
*/
class GTSAM_EXPORT Null : public Base {
public:
typedef boost::shared_ptr<Null> shared_ptr;
@ -146,7 +166,14 @@ class GTSAM_EXPORT Null : public Base {
}
};
/// Fair implements the "Fair" robust error model (Zhang97ivc)
/** Implementation of the "Fair" robust error model (Zhang97ivc)
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = c² (|x|/c - log(1+|x|/c))
* - Derivative \phi(x) = x/(1+|x|/c)
* - Weight w(x) = \phi(x)/x = 1/(1+|x|/c)
*/
class GTSAM_EXPORT Fair : public Base {
protected:
double c_;
@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double c, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
/** Serialization function */
@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base {
}
};
/// Huber implements the "Huber" robust error model (Zhang97ivc)
/** The "Huber" robust error model (Zhang97ivc).
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0.5 x² if |x|<k, 0.5 k² + k|x-k| otherwise
* - Derivative \phi(x) = x if |x|<k, k sgn(x) otherwise
* - Weight w(x) = \phi(x)/x = 1 if |x|<k, k/|x| otherwise
*/
class GTSAM_EXPORT Huber : public Base {
protected:
double k_;
@ -185,6 +220,7 @@ class GTSAM_EXPORT Huber : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
/** Serialization function */
@ -196,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
}
};
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
/// by:
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de
/// Thanks Jan!
/** Implementation of the "Cauchy" robust error model (Lee2013IROS).
* Contributed by:
* Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
* Information Technology, Karlsruhe, Germany.
* oberlaender@fzi.de
* Thanks Jan!
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0.5 k² log(1+x²/k²)
* - Derivative \phi(x) = (k²x)/(x²+k²)
* - Weight w(x) = \phi(x)/x = k²/(x²+k²)
*/
class GTSAM_EXPORT Cauchy : public Base {
protected:
double k_, ksquared_;
@ -215,6 +258,7 @@ class GTSAM_EXPORT Cauchy : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
/** Serialization function */
@ -227,7 +271,14 @@ class GTSAM_EXPORT Cauchy : public Base {
}
};
/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
/** Implementation of the "Tukey" robust error model (Zhang97ivc).
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|<c, c²/6 otherwise
* - Derivative \phi(x) = x(1-x²/c²)² if |x|<c, 0 otherwise
* - Weight w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c, 0 otherwise
*/
class GTSAM_EXPORT Tukey : public Base {
protected:
double c_, csquared_;
@ -241,6 +292,7 @@ class GTSAM_EXPORT Tukey : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
/** Serialization function */
@ -252,7 +304,14 @@ class GTSAM_EXPORT Tukey : public Base {
}
};
/// Welsch implements the "Welsch" robust error model (Zhang97ivc)
/** Implementation of the "Welsch" robust error model (Zhang97ivc).
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
* - Derivative \phi(x) = x exp(-x²/c²)
* - Weight w(x) = \phi(x)/x = exp(-x²/c²)
*/
class GTSAM_EXPORT Welsch : public Base {
protected:
double c_, csquared_;
@ -266,6 +325,7 @@ class GTSAM_EXPORT Welsch : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
/** Serialization function */
@ -278,12 +338,16 @@ class GTSAM_EXPORT Welsch : public Base {
}
};
/// GemanMcClure implements the "Geman-McClure" robust error model
/// (Zhang97ivc).
///
/// Note that Geman-McClure weight function uses the parameter c == 1.0,
/// but here it's allowed to use different values, so we actually have
/// the generalized Geman-McClure from (Agarwal15phd).
/** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
*
* Note that Geman-McClure weight function uses the parameter c == 1.0,
* but here it's allowed to use different values, so we actually have
* the generalized Geman-McClure from (Agarwal15phd).
*
* - Loss \rho(x) = 0.5 (c²x²)/(c²+x²)
* - Derivative \phi(x) = xc⁴/(c²+x²)²
* - Weight w(x) = \phi(x)/x = c⁴/(c²+x²)²
*/
class GTSAM_EXPORT GemanMcClure : public Base {
public:
typedef boost::shared_ptr<GemanMcClure> shared_ptr;
@ -295,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected:
double c_;
@ -309,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
}
};
/// DCS implements the Dynamic Covariance Scaling robust error model
/// from the paper Robust Map Optimization (Agarwal13icra).
///
/// Under the special condition of the parameter c == 1.0 and not
/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
/** DCS implements the Dynamic Covariance Scaling robust error model
* from the paper Robust Map Optimization (Agarwal13icra).
*
* Under the special condition of the parameter c == 1.0 and not
* forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
*
* This model has a scalar parameter "c" (with "units" of squared error).
*
* - Loss \rho(x) = (c²x² + cx⁴)/(x²+c)² (for any "x")
* - Derivative \phi(x) = 2c²x/(x²+c)²
* - Weight w(x) = \phi(x)/x = 2c²/(x²+c)² if x²>c, 1 otherwise
*/
class GTSAM_EXPORT DCS : public Base {
public:
typedef boost::shared_ptr<DCS> shared_ptr;
@ -325,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected:
double c_;
@ -339,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
}
};
/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
/// width 2*k, centered at the origin. The resulting penalty within the dead
/// zone is always zero, and grows quadratically outside the dead zone. In this
/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
/// robust to outliers. This penalty can be used to create barrier functions in
/// a general way.
/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
* width 2*k, centered at the origin. The resulting penalty within the dead
* zone is always zero, and grows quadratically outside the dead zone. In this
* sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
* robust to outliers. This penalty can be used to create barrier functions in
* a general way.
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0 if |x|<k, 0.5(k-|x|)² otherwise
* - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k, (k+x) if x<-k
* - Weight w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k, (k+x)/x if x<-k
*/
class GTSAM_EXPORT L2WithDeadZone : public Base {
protected:
double k_;
@ -358,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
/** Serialization function */

4
gtsam/linear/linear.i
View File

@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
BlockJacobiPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();

6
gtsam/navigation/AHRSFactor.h
View File

@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/**
* Add a single Gyroscope measurement to the preintegration.
* @param measureOmedga Measured angular velocity (in body frame)
* Measurements are taken to be in the sensor
* frame and conversion to the body frame is handled by `body_P_sensor` in
* `PreintegratedRotationParams` (if provided).
*
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time step
*/
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);

7
gtsam/navigation/CombinedImuFactor.h
View File

@ -208,8 +208,11 @@ public:
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the
* sensor)
* Both accelerometer and gyroscope measurements are taken to be in the sensor
* frame and conversion to the body frame is handled by `body_P_sensor` in
* `PreintegrationParams`.
*
* @param measuredAcc Measured acceleration (as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between two consecutive IMU measurements
*/

6
gtsam/navigation/ImuFactor.h
View File

@ -121,7 +121,11 @@ public:
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* Both accelerometer and gyroscope measurements are taken to be in the sensor
* frame and conversion to the body frame is handled by `body_P_sensor` in
* `PreintegrationParams`.
*
* @param measuredAcc Measured acceleration (as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement
*/

5
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -27,7 +27,6 @@
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative
@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>(
Matrix expectedH = numericalDerivative11<Vector, NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative

23
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) {
Matrix H;
Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
}
@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
// MagFactor1
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
// MagFactor2
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7));
// MagFactor2
// MagFactor3
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //

16
gtsam/nonlinear/nonlinear.i
View File

@ -226,6 +226,10 @@ class Values {
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
void insert(size_t j, double c);
@ -269,6 +273,10 @@ class Values {
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector);
@ -310,6 +318,10 @@ class Values {
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
void insert_or_assign(size_t j, Vector vector);
@ -351,6 +363,10 @@ class Values {
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,

8
gtsam/sfm/SfmData.h
View File

@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
size_t numberCameras() const { return cameras.size(); }
/// The track formed by series of landmark measurements
SfmTrack track(size_t idx) const { return tracks[idx]; }
const SfmTrack& track(size_t idx) const { return tracks[idx]; }
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const { return cameras[idx]; }
const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
/// Getters
const std::vector<SfmCamera>& cameraList() const { return cameras; }
const std::vector<SfmTrack>& trackList() const { return tracks; }
/**
* @brief Create projection factors using keys i and P(j)

20
gtsam/sfm/sfm.i
View File

@ -9,6 +9,8 @@ class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
Point3 p;
double r;
double g;
@ -34,12 +36,15 @@ class SfmData {
static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename);
std::vector<gtsam::SfmTrack>& trackList() const;
std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const;
size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack& track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model =
@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
};
class BinaryMeasurementsRot3 {
BinaryMeasurementsRot3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in
@ -184,6 +196,10 @@ class ShonanAveraging2 {
};
class ShonanAveraging3 {
ShonanAveraging3(
const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
const gtsam::ShonanAveragingParameters3& parameters =
gtsam::ShonanAveragingParameters3());
ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3& parameters);

3
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
return result.at<T>(kKey);
}
template <class T,
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
template <class T>
T FindKarcherMean(const std::vector<T>& rotations) {
return FindKarcherMeanImpl(rotations);
}

1
gtsam/slam/dataset.h
View File

@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED

16
gtsam/slam/slam.i
View File

@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorCal3Unified;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
gtsam::Point3>
GeneralSFMFactorPoseCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorPoseCal3Unified;
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

18
python/CMakeLists.txt
View File

@ -48,6 +48,7 @@ set(ignore
gtsam::Rot3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::DiscreteKey
gtsam::KeyPairDoubleMap)
@ -99,11 +100,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
"${GTSAM_MODULE_PATH}")
# Hack to get python test files copied every time they are modified
# Hack to get python test and util files copied every time they are modified
file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
endforeach()
# Common directory for data/datasets stored with the package.
# This will store the data in the Python site package directly.
@ -126,6 +139,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3Unified
@ -161,7 +175,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
# Hack to get python test files copied every time they are modified
file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
endforeach()

4
python/gtsam/preamble/geometry.h
View File

@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(
gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);

16
python/gtsam/preamble/sfm.h
View File

@ -9,4 +9,18 @@
* automatic STL binding, such that the raw objects can be accessed in Python.
* Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++.
*/
*/
// Including <stl.h> can cause some serious hard-to-debug bugs!!!
// #include <pybind11/stl.h>
#include <pybind11/stl_bind.h>
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmTrack>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::SfmCamera>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);

1
python/gtsam/specializations/geometry.h
View File

@ -16,6 +16,7 @@ py::bind_vector<
m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(

15
python/gtsam/specializations/sfm.h
View File

@ -13,4 +13,19 @@
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<
std::vector<gtsam::SfmTrack> >(
m_, "SfmTracks");
py::bind_vector<
std::vector<gtsam::SfmCamera> >(
m_, "SfmCameras");
py::bind_vector<
std::vector<std::pair<size_t, gtsam::Point2>>>(
m_, "SfmMeasurementVector"
);

11
python/gtsam/tests/test_Cal3Unified.py
View File

@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase):
self.gtsamAssertEquals(z, np.zeros(2))
self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
Dcal = np.zeros((2, 10), order='F')
Dp = np.zeros((2, 2), order='F')
camera.calibrate(img_point, Dcal, Dp)
self.gtsamAssertEquals(Dcal, np.array(
[[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]]))
self.gtsamAssertEquals(Dp, np.array(
[[ 1., -0.],
[-0., 1.]]))
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation(self):
"""Estimate spatial point from image measurements"""

46
python/gtsam/tests/test_PinholeCamera.py Normal file
View File

@ -0,0 +1,46 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
PinholeCamera unit tests.
Author: Fan Jiang
"""
import unittest
from math import pi
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestPinholeCamera(GtsamTestCase):
"""
Tests if we can correctly get the camera Jacobians in Python
"""
def test_jacobian(self):
cam1 = gtsam.PinholeCameraCal3Bundler()
# order is important because Eigen is column major!
Dpose = np.zeros((2, 6), order='F')
Dpoint = np.zeros((2, 3), order='F')
Dcal = np.zeros((2, 3), order='F')
cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal)
self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))
self.gtsamAssertEquals(
Dpose,
np.array([
[1., -2., 1., -1., 0., 1.], #
[2., -1., -1., 0., -1., 1.]
]))
self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]]))
if __name__ == "__main__":
unittest.main()

194
python/gtsam/tests/test_Sim2.py Normal file
View File

@ -0,0 +1,194 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Sim3 unit tests.
Author: John Lambert
"""
# pylint: disable=no-name-in-module
import unittest
import numpy as np
from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2
from gtsam.utils.test_case import GtsamTestCase
class TestSim2(GtsamTestCase):
"""Test selected Sim2 methods."""
def test_align_poses_along_straight_line(self) -> None:
"""Test Align Pose2Pairs method.
Scenario:
3 object poses
same scale (no gauge ambiguity)
world frame has poses rotated about 180 degrees.
world and egovehicle frame translated by 15 meters w.r.t. each other
"""
R180 = Rot2.fromDegrees(180)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose2(Rot2(), np.array([5, 0]))
eTo1 = Pose2(Rot2(), np.array([10, 0]))
eTo2 = Pose2(Rot2(), np.array([15, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world "w" frame)
wTo0 = Pose2(R180, np.array([-10, 0]))
wTo1 = Pose2(R180, np.array([-5, 0]))
wTo2 = Pose2(R180, np.array([0, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario:
3 object poses
with gauge ambiguity (2x scale)
world frame has poses rotated by 90 degrees.
world and egovehicle frame translated by 11 meters w.r.t. each other
"""
R90 = Rot2.fromDegrees(90)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose2(Rot2(), np.array([1, 0]))
eTo1 = Pose2(Rot2(), np.array([2, 0]))
eTo2 = Pose2(Rot2(), np.array([4, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(R90, np.array([0, 12]))
wTo1 = Pose2(R90, np.array([0, 14]))
wTo2 = Pose2(R90, np.array([0, 18]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self):
"""Test if Align Pose2Pairs method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
Scenario:
4 object poses
with gauge ambiguity (2.5x scale)
"""
# 0, 90, 180, and 270 degrees yaw
R0 = Rot2.fromDegrees(0)
R90 = Rot2.fromDegrees(90)
R180 = Rot2.fromDegrees(180)
R270 = Rot2.fromDegrees(270)
aTi0 = Pose2(R0, np.array([2, 3]))
aTi1 = Pose2(R90, np.array([12, 3]))
aTi2 = Pose2(R180, np.array([12, 13]))
aTi3 = Pose2(R270, np.array([2, 13]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
bTi0 = Pose2(R0, np.array([4, 3]))
bTi1 = Pose2(R90, np.array([8, 3]))
bTi2 = Pose2(R180, np.array([8, 7]))
bTi3 = Pose2(R270, np.array([4, 7]))
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity2.Align(ab_pairs)
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
def test_constructor(self) -> None:
"""Sim(2) to perform p_b = bSa * p_a"""
bRa = Rot2()
bta = np.array([1, 2])
bsa = 3.0
bSa = Similarity2(R=bRa, t=bta, s=bsa)
self.assertIsInstance(bSa, Similarity2)
np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix())
np.testing.assert_allclose(bSa.translation(), bta)
np.testing.assert_allclose(bSa.scale(), bsa)
def test_is_eq(self) -> None:
"""Ensure object equality works properly (are equal)."""
bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
self.gtsamAssertEquals(bSa, bSa_)
def test_not_eq_translation(self) -> None:
"""Ensure object equality works properly (not equal translation)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
self.assertNotEqual(bSa, bSa_)
def test_not_eq_rotation(self) -> None:
"""Ensure object equality works properly (not equal rotation)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3)
self.assertNotEqual(bSa, bSa_)
def test_not_eq_scale(self) -> None:
"""Ensure object equality works properly (not equal scale)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0)
self.assertNotEqual(bSa, bSa_)
def test_rotation(self) -> None:
"""Ensure rotation component is returned properly."""
R = Rot2.fromDegrees(90)
t = np.array([1, 2])
bSa = Similarity2(R=R, t=t, s=3.0)
# evaluates to [[0, -1], [1, 0]]
expected_R = Rot2.fromDegrees(90)
np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix())
def test_translation(self) -> None:
"""Ensure translation component is returned properly."""
R = Rot2.fromDegrees(90)
t = np.array([1, 2])
bSa = Similarity2(R=R, t=t, s=3.0)
expected_t = np.array([1, 2])
np.testing.assert_allclose(expected_t, bSa.translation())
def test_scale(self) -> None:
"""Ensure the scale factor is returned properly."""
bRa = Rot2()
bta = np.array([1, 2])
bsa = 3.0
bSa = Similarity2(R=bRa, t=bta, s=bsa)
self.assertEqual(bSa.scale(), 3.0)
if __name__ == "__main__":
unittest.main()