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

Merged in feature/typedefPoint3 (pull request #225) 

Point3 -> Vector3
release/4.3a0
Frank Dellaert 2016-02-18 11:13:33 -08:00
parent f6eb67e869 6d4cf0c115
commit 5afdc24ca7
79 changed files with 447 additions and 511 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
CMakeLists.txt
View File

@ -66,6 +66,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -85,7 +86,11 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
@ -478,6 +483,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

27
gtsam/base/serializationTestHelpers.h
View File

@ -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)
@ -11,7 +11,7 @@
/**
* @file serializationTestHelpers.h
* @brief
* @brief
* @author Alex Cunningham
* @author Richard Roberts
* @date Feb 7, 2012
@ -30,6 +30,11 @@ const bool verbose = false;
namespace gtsam {
namespace serializationTestHelpers {
// templated default object creation so we only need to declare one friend (if applicable)
template<class T>
T create() {
return T();
}
// Templated round-trip serialization
template<class T>
@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input==output;
}
@ -52,7 +57,7 @@ bool equality(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsObj(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return assert_equal(input, output);
}
@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) {
// De-referenced version for pointers, requires equals method
template<class T>
bool equalsDereferenced(const T& input) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input->equals(*output);
}
@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input==output;
}
@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return assert_equal(input, output);
}
@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input->equals(*output);
}
@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input==output;
}
@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return assert_equal(input, output);
}
@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input->equals(*output);
}

3
gtsam/config.h.in
View File

@ -62,3 +62,6 @@
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS

2
gtsam/geometry/EssentialMatrix.cpp
View File

@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) {
Rot3 R = E.rotation();
Unit3 d = E.direction();
os.precision(10);
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
return os;
}

4
gtsam/geometry/OrientedPlane3.cpp
View File

@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = zeros(3, 6);
@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;

141
gtsam/geometry/Point3.cpp
View File

@ -15,63 +15,50 @@
*/
#include <gtsam/geometry/Point3.h>
#include <cmath>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
&& fabs(z_ - q.z()) < tol);
#ifndef GTSAM_USE_VECTOR3_POINTS
bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol);
}
/* ************************************************************************* */
void Point3::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
double Point3::distance(const Point3 &q, 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);
}
return gtsam::distance(*this,q,H1,H2);
}
if (H2) {
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
*H2 = *H2 *(1. / d);
}
return d;
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
return gtsam::normalize(*this, H);
}
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
return gtsam::cross(*this, q, H1, H2);
}
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::dot(*this, q, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
}
/* ************************************************************************* */
@ -91,60 +78,58 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
}
#endif
#endif
/* ************************************************************************* */
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());
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double d = (q - p1).norm();
if (H1) {
*H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
*H1 = *H1 *(1. / d);
}
if (H_q) {
*H_q << skewSymmetric(vector());
if (H2) {
*H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
*H2 = *H2 *(1. / d);
}
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
return d;
}
/* ************************************************************************* */
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_);
}
/* ************************************************************************* */
double Point3::norm(OptionalJacobian<1,3> H) const {
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
*H << p.x() / r, p.y() / r, p.z() / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
Point3 normalized = p / norm(p);
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
*H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) {
if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),
p.x() * q.y() - p.y() * q.x());
}
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();
if (H2) *H2 << p.x(), p.y(), p.z();
return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
}
/* ************************************************************************* */

136
gtsam/geometry/Point3.h
View File

@ -21,46 +21,47 @@
#pragma once
#include <gtsam/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
namespace gtsam {
/**
* A 3D point
#ifdef GTSAM_USE_VECTOR3_POINTS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
#else
/**
* A 3D point is just a Vector3 with some additional methods
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 {
private:
double x_, y_, z_;
class GTSAM_EXPORT Point3 : public Vector3 {
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor creates a zero-Point3
Point3(): x_(0), y_(0), z_(0) {}
#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Default constructor no longer initializes, just like Vector3
Point3() {}
#endif
/// Construct from x, y, and z coordinates
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
Point3(double x, double y, double z): Vector3(x,y, z) {}
/// @}
/// @name Advanced Constructors
/// @{
/// Construct from 3-element vector
explicit Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
}
/// Construct from other vector
template<typename Derived>
inline Point3(const Eigen::MatrixBase<Derived>& v): Vector3(v) {}
/// @}
/// @name Testable
@ -77,32 +78,12 @@ namespace gtsam {
/// @{
/// identity for group operation
inline static Point3 identity() { return Point3();}
/// 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;
/// subtract
Point3 operator - (const Point3& q) const;
inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
/// @}
/// @name Vector Space
/// @{
///multiply with a scalar
Point3 operator * (double s) const;
///divide by a scalar
Point3 operator / (double s) const;
/** distance between two points */
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
@ -111,7 +92,7 @@ namespace gtsam {
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@ -125,20 +106,17 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// equality
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector3 vector() const { return Vector3(x_,y_,z_); }
/// return as Vector3
const Vector3& vector() const { return *this; }
/// get x
inline double x() const {return x_;}
inline double x() const {return (*this)[0];}
/// get y
inline double y() const {return y_;}
inline double y() const {return (*this)[1];}
/// get z
inline double z() const {return z_;}
inline double z() const {return (*this)[2];}
/// @}
@ -148,6 +126,7 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3() { setZero(); } // initializes to zero, in contrast to new behavior
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
@ -155,7 +134,8 @@ namespace gtsam {
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
inline double dist(const Point3& p2) const { return (p2 - *this).norm(); }
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
@ -165,35 +145,51 @@ namespace gtsam {
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
}
/// @}
};
// 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;}
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points
double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
/// cross product @return this x q
Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none,
OptionalJacobian<3, 3> H_q = boost::none);
/// dot product
double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
template <typename A1, typename A2>
struct Range;
@ -203,7 +199,7 @@ struct Range<Point3, Point3> {
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
return distance(p, q, H1, H2);
}
};

58
gtsam/geometry/Pose3.cpp
View File

@ -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)
@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const {
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const {
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R;
Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
Matrix6 adj;
adj << R, Z_3x3, A, R;
return adj;
@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
void Pose3::print(const string& s) const {
cout << s;
R_.print("R:\n");
t_.print("t: ");
cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
}
/* ************************************************************************* */
@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi
Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Rot3 R = Rot3::Expmap(omega.vector());
Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm();
const Vector3 w = Rot3::Logmap(p.rotation());
const Vector3 T = p.translation();
const double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
Matrix3 W = skewSymmetric(w / t);
const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5 * t);
Vector3 WT = W * T;
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
const double Tan = tan(0.5 * t);
const Vector3 WT = W * T;
const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
H->topLeftCorner<3,3>() = DR;
}
Vector6 xi;
xi << omega, T.translation().vector();
xi << omega, T.translation();
return xi;
#endif
}
@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
Matrix4 Pose3::matrix() const {
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
Matrix4 mat;
mat << R_.matrix(), t_.vector(), A14;
mat << R_.matrix(), t_, A14;
return mat;
}
@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
if (Dpoint) {
*Dpoint = R;
}
return Point3(R * p.vector()) + t_;
return R_ * p + t_;
}
/* ************************************************************************* */
@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
const Point3 q(Rt*(p - t_));
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
(*Dpose) <<
@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm();
} else {
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
const double r = norm(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
@ -361,10 +361,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
return boost::none; // we need at least three pairs
// calculate centroids
Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
Point3 cp(0,0,0), cq(0,0,0);
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector();
cq += pair.second.vector();
cp += pair.first;
cq += pair.second;
}
double f = 1.0 / n;
cp *= f;
@ -373,10 +373,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
// Add to form H matrix
Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector3 dp = pair.first.vector() - cp;
Vector3 dq = pair.second.vector() - cq;
Point3 dp = pair.first - cp;
Point3 dq = pair.second - cq;
H += dp * dq.transpose();
}
}
// Compute SVD
Matrix U, V;
@ -394,7 +394,9 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
os << pose.rotation() << "\n" << pose.translation() << endl;
os << pose.rotation() << "\n";
const Point3& t = pose.translation();
os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
return os;
}

10
gtsam/geometry/Pose3.h
View File

@ -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)
@ -52,8 +52,7 @@ public:
/// @{
/** Default constructor is origin */
Pose3() {
}
Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
/** Copy constructor */
Pose3(const Pose3& pose) :
@ -65,11 +64,6 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);

4
gtsam/geometry/Rot3.cpp
View File

@ -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)
@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
Point3 q(Rt * p); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;

49
gtsam/geometry/Rot3.h
View File

@ -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)
@ -93,7 +93,7 @@ namespace gtsam {
* See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
*/
template<typename Derived>
inline Rot3(const Eigen::MatrixBase<Derived>& R) {
inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=Matrix3(R);
#else
@ -105,7 +105,7 @@ namespace gtsam {
* Constructor from a rotation matrix
* Overload version for Matrix3 to avoid casting in quaternion mode.
*/
inline Rot3(const Matrix3& R) {
inline explicit Rot3(const Matrix3& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=R;
#else
@ -171,20 +171,6 @@ namespace gtsam {
return Rot3(q);
}
/**
* Convert from axis/angle representation
* @param axis is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::AxisAngle(axis,angle);
#endif
}
/**
* Convert from axis/angle representation
* @param axisw is the rotation axis, unit length
@ -192,7 +178,11 @@ namespace gtsam {
* @return incremental rotation
*/
static Rot3 AxisAngle(const Point3& axis, double angle) {
return AxisAngle(axis.vector(),angle);
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif
}
/**
@ -315,7 +305,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return traits<SO3>::Expmap(v);
return Rot3(traits<SO3>::Expmap(v));
#endif
}
@ -352,17 +342,6 @@ namespace gtsam {
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// operator* for Vector3
inline Vector3 operator*(const Vector3& v) const {
return rotate(Point3(v)).vector();
}
/// rotate for Vector3
Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return rotate(Point3(v), H1, H2).vector();
}
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
@ -370,12 +349,6 @@ namespace gtsam {
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// unrotate for Vector3
Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return unrotate(Point3(v), H1, H2).vector();
}
/// @}
/// @name Group Action on Unit3
/// @{
@ -483,7 +456,9 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
#ifndef GTSAM_USE_VECTOR3_POINTS
static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
#endif
static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
@ -539,7 +514,7 @@ namespace gtsam {
template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}

10
gtsam/geometry/Rot3M.cpp
View File

@ -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)
@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector();
rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector();
rot_.col(0) = (Vector3)col1;
rot_.col(1) = (Vector3)col2;
rot_.col(2) = (Vector3)col3;
}
/* ************************************************************************* */
@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
return Point3(rot_ * p.vector());
return rot_ * p;
}
/* ************************************************************************* */

29
gtsam/geometry/Unit3.cpp
View File

@ -35,6 +35,7 @@
#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <limits>
#include <cmath>
using namespace std;
@ -44,9 +45,8 @@ namespace gtsam {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
// 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();
direction.p_ = normalize(point, H ? &D_p_point : 0);
if (H)
*H << direction.basis().transpose() * D_p_point;
return direction;
@ -89,31 +89,27 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
const Point3 n(p_);
// Get the axis of rotation with the minimum projected length of the point
Point3 axis;
Point3 axis(0, 0, 1);
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);
}
// 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);
Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0);
// Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix33 H_b1_B1;
Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0);
Point3 b1 = normalize(B1, 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);
Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0);
// Create the basis by stacking b1 and b2.
B_.reset(Matrix32());
@ -175,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
// 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);
double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0);
if (H_p) {
(*H_p) << H_dot_pn * H_pn_p;
@ -208,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
// 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();
Vector2 xi = Bt * qn;
if (H_p) {
// Derivatives of each basis vector.
@ -216,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
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();
Matrix13 H_xi1_b1 = qn.transpose();
Matrix13 H_xi2_b2 = qn.transpose();
// Assemble dxi/dp = dxi/dB * dB/dp.
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
@ -252,11 +248,10 @@ Unit3 Unit3::retract(const Vector2& v) const {
// Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) {
return Unit3(cos(theta) * p_ + xi_hat);
return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
}
Vector3 exp_p_xi_hat =
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat);
}

2
gtsam/geometry/Unit3.h
View File

@ -65,10 +65,12 @@ public:
p_(1.0, 0.0, 0.0) {
}
#ifndef GTSAM_USE_VECTOR3_POINTS
/// Construct from point
explicit Unit3(const Point3& p) :
p_(p.vector().normalized()) {
}
#endif
/// Construct from a vector3
explicit Unit3(const Vector3& p) :

18
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -103,16 +103,16 @@ TEST( PinholeCamera, level2)
TEST( PinholeCamera, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
EXPECT(assert_equal(camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -149,7 +149,7 @@ TEST( PinholeCamera, backprojectInfinity)
/* ************************************************************************* */
TEST( PinholeCamera, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -165,7 +165,7 @@ TEST( PinholeCamera, backproject2)
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -180,7 +180,7 @@ TEST( PinholeCamera, backprojectInfinity2)
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity3)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K);
@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) {
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));

13
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) {
TEST( PinholePose, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity)
/* ************************************************************************* */
TEST( PinholePose, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -212,8 +212,7 @@ TEST( PinholePose, range0) {
double result = camera.range(point1, D1, D2);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9);
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}

24
gtsam/geometry/tests/testPoint3.cpp
View File

@ -71,34 +71,36 @@ TEST( Point3, arithmetic) {
/* ************************************************************************* */
TEST( Point3, equals) {
CHECK(P.equals(P));
Point3 Q;
CHECK(!P.equals(Q));
CHECK(traits<Point3>::Equals(P,P));
Point3 Q(0,0,0);
CHECK(!traits<Point3>::Equals(P,Q));
}
/* ************************************************************************* */
TEST( Point3, dot) {
Point3 origin, ones(1, 1, 1);
Point3 origin(0,0,0), ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2);
}
/* ************************************************************************* */
#ifndef GTSAM_USE_VECTOR3_POINTS
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "[1, 2, -3]';");
}
#endif
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point
Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(&Point3::normalize, _1, boost::none), point);
boost::bind(gtsam::normalize, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
@ -111,20 +113,20 @@ TEST (Point3, norm) {
Matrix actualH;
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q);
return distance(P,Q);
}
TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = P.distance(Q, H1, H2);
double d = distance(P, Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);
@ -137,9 +139,9 @@ TEST (Point3, distance) {
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
omega.cross(theta, aH1, aH2);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -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)
@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
}
@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap)
{
Vector v = zero(6); v(0) = 0.3;
Pose3 pose = Pose3::Expmap(v);
EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2));
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
}
@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}
@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}
@ -306,7 +306,7 @@ TEST( Pose3, Dtransform_from1_b)
TEST( Pose3, Dtransform_from1_c)
{
Point3 origin;
Point3 origin(0,0,0);
Pose3 T0(R,origin);
Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none);
@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate)
/* ************************************************************************* */
TEST( Pose3, transform_to_rotate)
{
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3());
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0));
Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10);
EXPECT(assert_equal(expected, actual, 0.001));
@ -406,7 +406,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */
TEST( Pose3, transform_from)
{
Point3 actual = T3.transform_from(Point3());
Point3 actual = T3.transform_from(Point3(0,0,0));
Point3 expected = Point3(1.,2.,3.);
EXPECT(assert_equal(expected, actual));
}

4
gtsam/geometry/tests/testRot3.cpp
View File

@ -542,8 +542,8 @@ TEST(Rot3, quaternion) {
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
Point3 actual1 = Point3(q1*p1);
Point3 actual2 = Point3(q2*p2);
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));

10
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -63,16 +63,16 @@ TEST( SimpleCamera, level2)
TEST( SimpleCamera, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -100,7 +100,7 @@ TEST( SimpleCamera, backproject)
/* ************************************************************************* */
TEST( SimpleCamera, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(Pose3(rot, origin), K);

2
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -46,7 +46,7 @@ TEST( StereoCamera, project)
// point X Y Z in meters
Point3 p(0, 0, 5);
StereoPoint2 result = stereoCam.project(p);
CHECK(assert_equal(StereoPoint2(320,320-150,240),result));
CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result));
// point X Y Z in meters
Point3 p2(1, 1, 5);

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) {
TEST(Unit3, Random) {
boost::mt19937 rng(42);
// Check that means are all zero at least
Point3 expectedMean, actualMean;
Point3 expectedMean(0,0,0), actualMean(0,0,0);
for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Unit3::Random(rng).point3();
actualMean = actualMean / 100;

2
gtsam/geometry/triangulation.h
View File

@ -436,7 +436,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
BOOST_FOREACH(const CAMERA& camera, cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& pose.translation().distance(point)
&& distance(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION

16
gtsam/linear/NoiseModel.cpp
View File

@ -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)
@ -82,13 +82,13 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
size_t m = R.rows(), n = R.cols();
if (m != n)
throw invalid_argument("Gaussian::SqrtInformation: R not square");
boost::optional<Vector> diagonal = boost::none;
if (smart)
diagonal = checkIfDiagonal(R);
if (diagonal)
return Diagonal::Sigmas(diagonal->array().inverse(), true);
else
return shared_ptr(new Gaussian(R.rows(), R));
if (smart) {
boost::optional<Vector> diagonal = checkIfDiagonal(R);
if (diagonal)
return Diagonal::Sigmas(diagonal->array().inverse(), true);
}
// NOTE(frank): only reaches here if !smart && !diagonal
return shared_ptr(new Gaussian(R.rows(), R));
}
/* ************************************************************************* */

8
gtsam/navigation/GPSFactor.cpp
View File

@ -25,14 +25,14 @@ namespace gtsam {
//***************************************************************************
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
nT_.print(" prior mean: ");
cout << " prior mean: " << nT_ << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p,
H->block < 3, 3 > (0, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix();
}
return p.translation().vector() -nT_.vector();
return p.translation() -nT_;
}
//***************************************************************************
@ -66,7 +66,7 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
// Construct initial pose
Pose3 nTb(nRb, nT); // nTb
return make_pair(nTb, nV.vector());
return make_pair(nTb, nV);
}
//***************************************************************************

5
gtsam/navigation/GPSFactor.h
View File

@ -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)
@ -48,8 +48,7 @@ public:
typedef GPSFactor This;
/** default constructor - only use for serialization */
GPSFactor() {
}
GPSFactor(): nT_(0, 0, 0) {}
virtual ~GPSFactor() {
}

10
gtsam/navigation/MagFactor.h
View File

@ -76,7 +76,7 @@ public:
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRb<52> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_).vector();
return (hx - measured_);
}
};
@ -114,7 +114,7 @@ public:
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRb<52> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector();
return (hx - measured_);
}
};
@ -155,7 +155,7 @@ public:
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
*H2 = eye(3);
return (hx - measured_).vector();
return (hx - measured_);
}
};
@ -197,7 +197,7 @@ public:
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Point3 hx = scale * rotated.point3() + bias;
if (H1)
*H1 = rotated.point3().vector();
*H1 = rotated.point3();
if (H2) // H2 is 2*2, but we need 3*2
{
Matrix H;
@ -206,7 +206,7 @@ public:
}
if (H3)
*H3 = eye(3);
return (hx - measured_).vector();
return (hx - measured_);
}
};

13
gtsam/navigation/NavState.cpp
View File

@ -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)
@ -89,7 +89,7 @@ void NavState::print(const string& s) const {
//------------------------------------------------------------------------------
bool NavState::equals(const NavState& other, double tol) const {
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
&& equal_with_abs_tol(v_, other.v_, tol);
}
@ -121,11 +121,6 @@ Point3 NavState::operator*(const Point3& b_t) const {
return Point3(R_ * b_t + t_);
}
//------------------------------------------------------------------------------
Velocity3 NavState::operator*(const Velocity3& b_v) const {
return Velocity3(R_ * b_v + v_);
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
OptionalJacobian<9, 9> H) {
@ -189,7 +184,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p.vector();
Vector3 n_t = n_p;
// NOTE(frank): See Pose3::Logmap
Vector9 xi;
@ -300,7 +295,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(xi) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector()));
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t;
}

7
gtsam/navigation/NavState.h
View File

@ -49,7 +49,7 @@ public:
/// Default constructor
NavState() :
v_(Vector3::Zero()) {
t_(0,0,0), v_(Vector3::Zero()) {
}
/// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@ -97,7 +97,7 @@ public:
}
/// Return position as Vector3
Vector3 t() const {
return t_.vector();
return t_;
}
/// Return velocity as Vector3. Computation-free.
const Vector3& v() const {
@ -147,9 +147,6 @@ public:
/// Act on position alone, n_t = nRb * b_t + n_t
Point3 operator*(const Point3& b_t) const;
/// Act on velocity alone, n_v = nRb * b_v + n_v
Velocity3 operator*(const Velocity3& b_v) const;
/// @}
/// @name Manifold
/// @{

4
gtsam/navigation/PreintegrationBase.cpp
View File

@ -94,7 +94,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector();
const Vector3 b_arm = p().body_P_sensor->translation();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the unbiased one
// to get linear acceleration vector in the body frame:
@ -188,7 +188,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().vector().isZero())
if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}

2
gtsam/navigation/Scenario.h
View File

@ -80,7 +80,7 @@ class AcceleratingScenario : public Scenario {
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& a_n,
const Vector3& omega_b = Vector3::Zero())
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
: nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
Pose3 pose(double t) const override {
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);

4
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -590,7 +590,7 @@ TEST(ImuFactor, PredictRotation) {
// Predict
NavState actual = pim.predict(NavState(), bias);
NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1);
NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1);
EXPECT(assert_equal(expected, actual));
}
@ -708,7 +708,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
auto p = testing::Params();
p->n_gravity = Vector3(0, 0, -kGravity);
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3());
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3;

2
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -43,7 +43,7 @@ TEST(ImuFactor, serialization) {
// Create default parameters with Z-down and above noise paramaters
auto p = PreintegrationParams::MakeSharedD(9.81);
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3());
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3;

2
gtsam/navigation/tests/testPoseVelocityBias.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
Matrix3 R1 = pvb1.pose.rotation().matrix();
// Ri.transpose() translate the error from the global frame into pose1's frame
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector();
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation());
const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity);
const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation());
const Vector3 fR = Rot3::Logmap(R1BetweenR2);

4
gtsam/navigation/tests/testScenarios.cpp
View File

@ -140,7 +140,7 @@ TEST(ScenarioRunner, Loop) {
/* ************************************************************************* */
namespace initial {
const Rot3 nRb;
const Point3 P0;
const Point3 P0(0,0,0);
const Vector3 V0(0, 0, 0);
}
@ -259,7 +259,7 @@ namespace initial3 {
// Rotation only
// Set up body pointing towards y axis. The body itself has Z axis pointing down
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
const Point3 P0;
const Point3 P0(0,0,0);
const Vector3 V0(0, 0, 0);
}

6
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -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)
@ -84,9 +84,9 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
} else if (const GenericValue<Point2>* p = dynamic_cast<const GenericValue<Point2>*>(&value)) {
t << p->value().x(), p->value().y(), 0;
} else if (const GenericValue<Pose3>* p = dynamic_cast<const GenericValue<Pose3>*>(&value)) {
t = p->value().translation().vector();
t = p->value().translation();
} else if (const GenericValue<Point3>* p = dynamic_cast<const GenericValue<Point3>*>(&value)) {
t = p->value().vector();
t = p->value();
} else {
return boost::none;
}

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Cal3Bundler0(1, 0, 0));
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point.vector();
Vector3 X = point;
Vector2 expectedMeasurement(1.2431567, 1.2525694);
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);

4
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -81,7 +81,7 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p.vector();
return p;
}
Expression<Point3> p(1);
set<Key> expected = list_of(1);
@ -108,7 +108,7 @@ TEST(Expression, NullaryMethod) {
// Create expression
Expression<Point3> p(67);
Expression<double> norm(p, &Point3::norm);
Expression<double> norm(&gtsam::norm, p);
// Create Values
Values values;

12
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -361,10 +361,10 @@ TEST( RangeFactor, Camera) {
namespace gtsam{
template <>
struct Range<Vector3, Vector3> {
struct Range<Vector4, Vector4> {
typedef double result_type;
double operator()(const Vector3& v1, const Vector3& v2,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
double operator()(const Vector4& v1, const Vector4& v2,
OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) {
return (v2 - v1).norm();
// derivatives not implemented
}
@ -376,11 +376,11 @@ TEST(RangeFactor, NonGTSAM) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor<Vector3> factor(poseKey, pointKey, measurement, model);
RangeFactor<Vector4> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Vector3 pose(1.0, 2.0, 00);
Vector3 point(-4.0, 11.0, 0);
Vector4 pose(1.0, 2.0, 00, 0);
Vector4 point(-4.0, 11.0, 0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();

17
gtsam/slam/EssentialMatrixFactor.h
View File

@ -19,7 +19,7 @@ namespace gtsam {
*/
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates
Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
typedef NoiseModelFactor1<EssentialMatrix> Base;
typedef EssentialMatrixFactor This;
@ -107,9 +107,7 @@ public:
*/
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model) :
Base(model, key1, key2) {
dP1_ = Point3(pA.x(), pA.y(), 1);
pn_ = pB;
Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) {
f_ = 1.0;
}
@ -125,11 +123,8 @@ public:
template<class CALIBRATION>
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
Base(model, key1, key2) {
assert(K);
Point2 p1 = K->calibrate(pA);
dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1)
pn_ = K->calibrate(pB);
Base(model, key1, key2), dP1_(
EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) {
f_ = 0.5 * (K->fx() + K->fy());
}
@ -144,7 +139,7 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose()
<< dP1_.transpose() << ")' and (" << pn_.vector().transpose()
<< ")'" << std::endl;
}
@ -196,7 +191,7 @@ public:
if (Dd) // efficient backwards computation:
// (2*3) * (3*3) * (3*1)
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector()));
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
}
Point2 reprojectionError = pn - pn_;

2
gtsam/slam/InitializePose3.cpp
View File

@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3());
Pose3 initializedPose = Pose3(rot, Point3(0,0,0));
initialPose.insert(key, initializedPose);
}
// add prior

6
gtsam/slam/PoseTranslationPrior.h
View File

@ -70,19 +70,19 @@ public:
(*H).middleCols(transInterval.first, tDim) = R.matrix();
}
return newTrans.vector() - measured_.vector();
return traits<Translation>::Local(measured_, newTrans);
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s + "PoseTranslationPrior", keyFormatter);
measured_.print("Measured Translation");
traits<Translation>::Print(measured_, "Measured Translation");
}
private:

6
gtsam/slam/RotateFactor.h
View File

@ -44,9 +44,9 @@ public:
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateFactor:" << std::endl;
p_.print("p");
z_.print("z");
std::cout << "RotateFactor:]\n";
std::cout << "p: " << p_.transpose() << std::endl;
std::cout << "z: " << z_.transpose() << std::endl;
}
/// vector of errors returns 2D vector

10
gtsam/slam/dataset.cpp
View File

@ -1,5 +1,5 @@
/* ----------------------------------------------------------------------------
* 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)
@ -779,7 +779,7 @@ bool writeBAL(const string& filename, SfM_data &data) {
os << endl;
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
SfM_Track track = data.tracks[j];
const SfM_Track& track = data.tracks[j];
for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
@ -808,7 +808,9 @@ bool writeBAL(const string& filename, SfM_data &data) {
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
os << poseOpenGL.translation().vector() << endl;
os << poseOpenGL.translation().x() << endl;
os << poseOpenGL.translation().y() << endl;
os << poseOpenGL.translation().z() << endl;
os << cameraCalibration.fx() << endl;
os << cameraCalibration.k1() << endl;
os << cameraCalibration.k2() << endl;
@ -880,7 +882,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
dataValues.tracks[j].r = 1.0;
dataValues.tracks[j].g = 0.0;
dataValues.tracks[j].b = 0.0;
dataValues.tracks[j].p = Point3();
dataValues.tracks[j].p = Point3(0,0,0);
}
}

3
gtsam/slam/dataset.h
View File

@ -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)
@ -147,6 +147,7 @@ typedef std::pair<size_t, size_t> SIFT_Index;
/// Define the structure for the 3D points
struct SfM_Track {
SfM_Track():p(0,0,0) {}
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))

4
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -154,7 +154,7 @@ TEST( GeneralSFMFactor, error ) {
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1;
Point3 l1(0,0,0);
values.insert(L(1), l1);
EXPECT(
assert_equal(((Vector ) Vector2(-3., 0.)),
@ -451,7 +451,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) {
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1;
Point3 l1(0,0,0);
values.insert(L(1), l1);
vector<SharedNoiseModel> models;

2
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -107,7 +107,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1;
Point3 l1(0,0,0);
values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
}

12
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -148,9 +148,9 @@ TEST( InitializePose3, iterationGradient ) {
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
size_t maxIter = 1; // test gradient at the first iteration
bool setRefFrame = false;
@ -189,9 +189,9 @@ TEST( InitializePose3, orientationsGradient ) {
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
// do 10 gradient iterations
bool setRefFrame = false;
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);

13
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -610,11 +610,11 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-3);
Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this)
Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this)
if (factor1->point())
oldPoint = *(factor1->point());
Point3 expectedPoint;
Point3 expectedPoint(0,0,0);
if (factor1->point(values))
expectedPoint = *(factor1->point(values));
@ -636,10 +636,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
smartGraph.push_back(factor1);
double expectedError = factor1->error(values);
double expectedErrorGraph = smartGraph.error(values);
Point3 expectedPoint;
Point3 expectedPoint(0,0,0);
if (factor1->point())
expectedPoint = *(factor1->point());
// cout << "expectedPoint " << expectedPoint.vector() << endl;
// COMMENTS:
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
@ -677,7 +676,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
smartGraph.push_back(factor1);
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
Point3 expectedPoint;
Point3 expectedPoint(0,0,0);
if (factor1->point())
expectedPoint = *(factor1->point());
@ -720,7 +719,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
// smartGraph.push_back(factor1);
// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
//
// Point3 expectedPoint;
// Point3 expectedPoint(0,0,0);
// if(factor1->point())
// expectedPoint = *(factor1->point());
//
@ -773,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
cameras.push_back(level_camera_right);
factor1->error(values); // this is important to have a triangulation of the point
Point3 point;
Point3 point(0,0,0);
if (factor1->point())
point = *(factor1->point());
vector<Matrix29> Fblocks;

4
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -959,8 +959,8 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
views.push_back(x3);
// Two different cameras, at the same position, but different rotations
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3());
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3());
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
Camera cam2(pose2, sharedK2);
Camera cam3(pose3, sharedK2);

4
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -87,7 +87,7 @@ public:
Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
@ -109,7 +109,7 @@ private:
static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
return hx;
}
};

10
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -37,7 +37,7 @@ PoseRTV::PoseRTV(const Vector& rtv) :
Vector PoseRTV::vector() const {
Vector rtv(9);
rtv.head(3) = rotation().xyz();
rtv.segment(3,3) = translation().vector();
rtv.segment(3,3) = translation();
rtv.tail(3) = velocity();
return rtv;
}
@ -52,7 +52,7 @@ bool PoseRTV::equals(const PoseRTV& other, double tol) const {
void PoseRTV::print(const string& s) const {
cout << s << ":" << endl;
gtsam::print((Vector)R().xyz(), " R:rpy");
t().print(" T");
cout << " T" << t().transpose() << endl;
gtsam::print((Vector)velocity(), " V");
}
@ -103,7 +103,7 @@ PoseRTV PoseRTV::flyingDynamics(
Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
Point3 forward(forward_accel, 0.0, 0.0);
Vector Acc_n =
yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame
yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame
- drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
+ delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch
@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other,
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
Matrix13 D_d_t1, D_d_t2;
double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
return d;
@ -188,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
// Note that we rotate the velocity
Matrix3 D_newvel_R, D_newvel_v;
Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
if (Dglobal) {
Dglobal->setZero();

4
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -66,7 +66,7 @@ public:
// Access to vector for ease of use with Matlab
// and avoidance of Point3
Vector vector() const;
Vector translationVec() const { return pose().translation().vector(); }
Vector translationVec() const { return pose().translation(); }
const Velocity3& velocityVec() const { return velocity(); }
// testable
@ -126,7 +126,7 @@ public:
/// @return a vector for Matlab compatibility
inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
return translationIntegration(x2, dt).vector();
return translationIntegration(x2, dt);
}
/**

4
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -108,14 +108,14 @@ private:
const Velocity3& v1 = x1.v(), v2 = x2.v();
const Point3& p1 = x1.t(), p2 = x2.t();
Point3 hx;
Point3 hx(0,0,0);
switch(mode) {
case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
default: assert(false); break;
}
return p2.vector() - hx.vector();
return p2 - hx;
}
};

2
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -29,7 +29,7 @@ TEST( testPoseRTV, constructors ) {
EXPECT(assert_equal(Pose3(rot, pt), state1.pose()));
PoseRTV state2;
EXPECT(assert_equal(Point3(), state2.t()));
EXPECT(assert_equal(Point3(0,0,0), state2.t()));
EXPECT(assert_equal(Rot3(), state2.R()));
EXPECT(assert_equal(kZero3, state2.v()));
EXPECT(assert_equal(Pose3(), state2.pose()));

3
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -89,7 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame
Vector fGravity_g1 = zero(6);
subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
Vector Fb = Fu+fGravity_g1;
Vector dV = newtonEuler(V1_g1, Fb, Inertia);

4
gtsam_unstable/geometry/BearingS2.cpp
View File

@ -34,7 +34,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) {
0.,1.,0.,
-1.,0.,0.).finished();
// p_rel_c = Cbc*Cnb*(PosObj - Pos);
Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector());
Vector p_rel_c = Cbc*Cnb*(B - A.translation());
// FIXME: the matlab code checks for p_rel_c(0) greater than
@ -50,7 +50,7 @@ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) {
// Cnb = DCMnb(Att);
Matrix Cnb = A.rotation().matrix().transpose();
Vector p_rel_c = Cnb*(B.vector() - A.translation().vector());
Vector p_rel_c = Cnb*(B - A.translation());
// FIXME: the matlab code checks for p_rel_c(0) greater than

9
gtsam_unstable/geometry/Event.h
View File

@ -35,7 +35,7 @@ public:
/// Default Constructor
Event() :
time_(0) {
time_(0), location_(0,0,0) {
}
/// Constructor from time and location
@ -60,14 +60,13 @@ public:
/** print with optional string */
void print(const std::string& s = "") const {
std::cout << s << "time = " << time_;
location_.print("; location");
std::cout << s << "time = " << time_ << "location = " << location_.transpose();
}
/** equals with an tolerance */
bool equals(const Event& other, double tol = 1e-9) const {
return std::abs(time_ - other.time_) < tol
&& location_.equals(other.location_, tol);
&& traits<Point3>::Equals(location_, other.location_, tol);
}
/// Updates a with tangent space delta
@ -86,7 +85,7 @@ public:
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = location_.distance(microphone, D1, D2);
double distance = gtsam::distance(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;

19
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -23,11 +23,11 @@
namespace gtsam {
Similarity3::Similarity3() :
R_(), t_(), s_(1) {
t_(0,0,0), s_(1) {
}
Similarity3::Similarity3(double s) :
s_(s) {
t_(0,0,0), s_(s) {
}
Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) :
@ -43,7 +43,7 @@ Similarity3::Similarity3(const Matrix4& T) :
}
bool Similarity3::equals(const Similarity3& other, double tol) const {
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
&& s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
}
@ -55,8 +55,7 @@ void Similarity3::print(const std::string& s) const {
std::cout << std::endl;
std::cout << s;
rotation().print("R:\n");
translation().print("t: ");
std::cout << "s: " << scale() << std::endl;
std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl;
}
Similarity3 Similarity3::identity() {
@ -79,7 +78,7 @@ Point3 Similarity3::transform_from(const Point3& p, //
// For this derivative, see LieGroups.pdf
const Matrix3 sR = s_ * R_.matrix();
const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 << DR, sR, sR * p.vector();
*H1 << DR, sR, sR * p;
}
if (H2)
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
@ -103,7 +102,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) {
Matrix7 Similarity3::AdjointMap() const {
// http://www.ethaneade.org/latex2html/lie/node30.html
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
const Vector3 t = t_;
const Matrix3 A = s_ * skewSymmetric(t) * R;
Matrix7 adj;
adj << R, Z_3x3, Matrix31::Zero(), // 3*7
@ -153,7 +152,7 @@ Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) {
const Vector3 w = Rot3::Logmap(T.R_);
const double lambda = log(T.s_);
Vector7 result;
result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda;
result << w, GetV(w, lambda).inverse() * T.t_, lambda;
if (Hm) {
throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
}
@ -173,13 +172,13 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
os << "[" << p.rotation().xyz().transpose() << " "
<< p.translation().vector().transpose() << " " << p.scale() << "]\';";
<< p.translation().transpose() << " " << p.scale() << "]\';";
return os;
}
const Matrix4 Similarity3::matrix() const {
Matrix4 T;
T.topRows<3>() << R_.matrix(), t_.vector();
T.topRows<3>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
return T;
}

4
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -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)
@ -34,7 +34,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0;
static const Point3 microphoneAt0(0,0,0);
//*****************************************************************************
TEST( Event, Constructor ) {

8
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -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)
@ -64,14 +64,14 @@ TEST(Similarity3, Constructors) {
Similarity3 sim3_Construct1;
Similarity3 sim3_Construct2(s);
Similarity3 sim3_Construct3(R, P, s);
Similarity3 sim4_Construct4(R.matrix(), P.vector(), s);
Similarity3 sim4_Construct4(R.matrix(), P, s);
}
//******************************************************************************
TEST(Similarity3, Getters) {
Similarity3 sim3_default;
EXPECT(assert_equal(Rot3(), sim3_default.rotation()));
EXPECT(assert_equal(Point3(), sim3_default.translation()));
EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation()));
EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9);
Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
@ -158,7 +158,7 @@ TEST( Similarity3, retract_first_order) {
Similarity3 id;
Vector v = zero(7);
v(0) = 0.3;
EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2));
EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
// v(3) = 0.2;
// v(4) = 0.7;
// v(5) = -2;

8
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -58,15 +58,15 @@ namespace gtsam {
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: ");
<< keyFormatter(this->key2()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
@ -82,7 +82,7 @@ namespace gtsam {
H2->resize(3,3); // jacobian wrt bias
(*H2) << Matrix3::Identity();
}
return pose.translation().vector() + bias.vector() - measured_.vector();
return pose.translation() + bias - measured_;
}
/** return the measured */

2
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -187,7 +187,7 @@ public:
Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector();
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation();
} else {
body_a_body = AccCorrected;
}

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -79,7 +79,7 @@ public:
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& this->referencePoint_.equals(e->referencePoint_, tol);
&& traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
}
Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {

8
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -1,12 +1,12 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
@ -253,7 +253,7 @@ public:
reprojections.push_back(cameras[i].backproject(measured_[i]));
}
Point3 pw_sum;
Point3 pw_sum(0,0,0);
BOOST_FOREACH(const Point3& pw, reprojections) {
pw_sum = pw_sum + pw;
}

12
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -233,7 +233,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
///* VADIM - START ************************************************************************* */
//Vector3 predictionRq(const Vector3 angles, const Vector3 q) {
// return (Rot3().RzRyRx(angles) * q).vector();
// return (Rot3().RzRyRx(angles) * q);
//}
//
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
@ -435,7 +435,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -474,7 +474,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -512,7 +512,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -554,7 +554,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -605,7 +605,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,

6
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

@ -48,7 +48,7 @@ TEST( InvDepthFactorVariant1, optimize) {
Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished());
// Create a factor graph with two inverse depth factors and two pose priors
Key poseKey1(1);
Key poseKey2(2);
@ -89,14 +89,14 @@ TEST( InvDepthFactorVariant1, optimize) {
// cout << endl << endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
Point3 world_landmarkBefore(0,0,0);
{
Vector6 landmarkBefore = values.at<Vector6>(landmarkKey);
double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
Point3 world_landmarkAfter;
Point3 world_landmarkAfter(0,0,0);
{
Vector6 landmarkAfter = result.at<Vector6>(landmarkKey);
double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);

8
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant2, optimize) {
Vector3 expected((Vector(3) << theta, phi, rho).finished());
// Create a factor graph with two inverse depth factors and two pose priors
Key poseKey1(1);
Key poseKey2(2);
@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant2, optimize) {
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
Vector3 actual = result.at<Vector3>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
// pose1.print("Pose1 Truth:\n");
@ -87,13 +87,13 @@ TEST( InvDepthFactorVariant2, optimize) {
// std::cout << std::endl << std::endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
Point3 world_landmarkBefore(0,0,0);
{
Vector3 landmarkBefore = values.at<Vector3>(landmarkKey);
double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
Point3 world_landmarkAfter;
Point3 world_landmarkAfter(0,0,0);
{
Vector3 landmarkAfter = result.at<Vector3>(landmarkKey);
double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);

2
gtsam_unstable/slam/tests/testTOAFactor.cpp
View File

@ -44,7 +44,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0;
static const Point3 microphoneAt0(0,0,0);
//*****************************************************************************
TEST( TOAFactor, NewWay ) {

32
matlab.h
View File

@ -28,8 +28,6 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/foreach.hpp>
#include <exception>
namespace gtsam {
@ -92,7 +90,7 @@ Matrix extractPoint2(const Values& values) {
size_t j = 0;
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points)
for(const auto& key_value: points)
result.row(j++) = key_value.value.vector();
return result;
}
@ -102,8 +100,8 @@ Matrix extractPoint3(const Values& values) {
Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(), 3);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector();
for(const auto& key_value: points)
result.row(j++) = key_value.value;
return result;
}
@ -112,7 +110,7 @@ Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(), 3);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses)
for(const auto& key_value: poses)
result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
return result;
}
@ -127,11 +125,11 @@ Matrix extractPose3(const Values& values) {
Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(), 12);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) {
for(const auto& key_value: poses) {
result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2);
result.row(j).tail(3) = key_value.value.translation().vector();
result.row(j).tail(3) = key_value.value.translation();
j++;
}
return result;
@ -142,8 +140,8 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
values.update(key_value.key, key_value.value + Point2(sampler.sample()));
for(const auto& key_value: values.filter<Point2>()) {
values.update<Point2>(key_value.key, key_value.value + Point2(sampler.sample()));
}
}
@ -153,8 +151,8 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
for(const auto& key_value: values.filter<Pose2>()) {
values.update<Pose2>(key_value.key, key_value.value.retract(sampler.sample()));
}
}
@ -163,8 +161,8 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
values.update(key_value.key, key_value.value + Point3(sampler.sample()));
for(const auto& key_value: values.filter<Point3>()) {
values.update<Point3>(key_value.key, key_value.value + Point3(sampler.sample()));
}
}
@ -204,13 +202,13 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
const Values& values) {
// first count
size_t K = 0, k = 0;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
for(const NonlinearFactor::shared_ptr& f: graph)
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f))
++K;
// now fill
Matrix errors(2, K);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
for(const NonlinearFactor::shared_ptr& f: graph) {
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f);
@ -232,7 +230,7 @@ Values localToWorld(const Values& local, const Pose2& base,
keys = local.keys();
// Loop over all keys
BOOST_FOREACH(Key key, keys) {
for(Key key: keys) {
try {
// if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key);

6
python/handwritten/geometry/Point3.cpp
View File

@ -41,12 +41,14 @@ class_<Point3>("Point3")
.def("dot", &Point3::dot)
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("norm", &Point3::norm)
.def("normalize", &Point3::normalize)
.def("normalized", &Point3::normalized)
.def("print", &Point3::print, print_overloads(args("s")))
.def("vector", &Point3::vector)
#ifndef GTSAM_USE_VECTOR3_POINTS
.def("vector", &Point3::vector, return_value_policy<copy_const_reference>())
.def("x", &Point3::x)
.def("y", &Point3::y)
.def("z", &Point3::z)
#endif
.def(self * other<double>())
.def(other<double>() * self)
.def(self + self)

10
python/handwritten/geometry/Rot3.cpp
View File

@ -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)
@ -37,8 +37,8 @@ static Rot3 Quaternion_1(double w, double x, double y, double z)
// Prototypes used to perform overloading
// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html
gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle;
gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle;
gtsam::Rot3 (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle;
gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle;
gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues;
gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues;
gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx;
@ -58,8 +58,6 @@ void exportRot3(){
.def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion")
.def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) )
.staticmethod("Quaternion")
.def("AxisAngle", AxisAngle_0)
.def("AxisAngle", AxisAngle_1)
.staticmethod("AxisAngle")
.def("Expmap", &Rot3::Expmap)
.staticmethod("Expmap")
@ -69,6 +67,8 @@ void exportRot3(){
.staticmethod("Logmap")
.def("LogmapDerivative", &Rot3::LogmapDerivative)
.staticmethod("LogmapDerivative")
.def("AxisAngle", AxisAngle_0)
.def("AxisAngle", AxisAngle_1)
.def("Rodrigues", Rodrigues_0)
.def("Rodrigues", Rodrigues_1)
.staticmethod("Rodrigues")

4
tests/simulated3D.h
View File

@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const {
return prior(x, H).vector() - measured_.vector();
return prior(x, H) - measured_;
}
};
@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return mea(x1, x2, H1, H2).vector() - measured_.vector();
return mea(x1, x2, H1, H2) - measured_;
}
};

2
tests/testExpressionFactor.cpp
View File

@ -139,7 +139,7 @@ TEST(ExpressionFactor, Unary) {
typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
Vector9 v;
v << p.vector(), p.vector(), p.vector();
v << p, p, p;
if (H) *H << eye(3), eye(3), eye(3);
return v;
}

2
tests/testNonlinearEquality.cpp
View File

@ -531,7 +531,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
// create initial estimates
Rot3 faceDownY(
(Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished());
Pose3 pose1(faceDownY, Point3()); // origin, left camera
Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera
SimpleCamera camera1(pose1, K);
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
SimpleCamera camera2(pose2, K);

9
timing/timeCalibratedCamera.cpp
View File

@ -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)
@ -27,12 +27,7 @@ int main()
{
int n = 100000;
const Pose3 pose1(Matrix3((Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished()),
Point3(0,0,0.5));
const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
const CalibratedCamera camera(pose1);
const Point3 point1(-0.08,-0.08, 0.0);

9
timing/timePinholeCamera.cpp
View File

@ -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)
@ -28,12 +28,7 @@ int main()
{
int n = 1e6;
const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
const PinholeCamera<Cal3Bundler> camera(pose1,K);

9
timing/timeStereoCamera.cpp
View File

@ -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)
@ -27,12 +27,7 @@ int main()
{
int n = 100000;
const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
const StereoCamera camera(pose1, K);

2
wrap/Class.cpp
View File

@ -567,7 +567,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
// string serialized = unwrap< string >(in[0]);
// istringstream in_archive_stream(serialized);
// boost::archive::text_iarchive in_archive(in_archive_stream);
// Shared output(new Point3());
// Shared output(new Point3(0,0,0));
// in_archive >> *output;
// out[0] = wrap_shared_ptr(output,"Point3", false);
//}