Now allows for a flag to compile Point3 as derived from Vector3
Frank
parent
3052afe42b
commit
3394e85ef7
|
|
@ -15,25 +15,25 @@
|
|||
*/
|
||||
|
||||
#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);
|
||||
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;
|
||||
}
|
||||
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
/* ************************************************************************* */
|
||||
|
||||
bool Point3::operator==(const Point3& q) const {
|
||||
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
|
||||
}
|
||||
|
|
@ -57,18 +57,19 @@ Point3 Point3::operator*(double s) const {
|
|||
Point3 Point3::operator/(double s) const {
|
||||
return Point3(x_ / s, y_ / s, z_ / s);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
double d = (p2 - *this).norm();
|
||||
if (H1) {
|
||||
*H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
|
||||
*H1 << x() - p2.x(), y() - p2.y(), z() - p2.z();
|
||||
*H1 = *H1 *(1. / d);
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
|
||||
*H2 << -x() + p2.x(), -y() + p2.y(), -z() + p2.z();
|
||||
*H2 = *H2 *(1. / d);
|
||||
}
|
||||
return d;
|
||||
|
|
@ -100,8 +101,8 @@ Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobi
|
|||
*H_q << skewSymmetric(vector());
|
||||
}
|
||||
|
||||
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
|
||||
x_ * q.y_ - y_ * q.x_);
|
||||
return Point3(y() * q.z() - z() * q.y(), z() * q.x() - x() * q.z(),
|
||||
x() * q.y() - y() * q.x());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -113,15 +114,15 @@ double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian
|
|||
*H_q << vector().transpose();
|
||||
}
|
||||
|
||||
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
|
||||
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 r = std::sqrt(x() * x() + y() * y() + z() * z());
|
||||
if (H) {
|
||||
if (fabs(r) > 1e-10)
|
||||
*H << x_ / r, y_ / r, z_ / r;
|
||||
*H << x() / r, y() / r, z() / r;
|
||||
else
|
||||
*H << 1, 1, 1; // really infinity, why 1 ?
|
||||
}
|
||||
|
|
@ -133,10 +134,10 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
|
|||
Point3 normalized = *this / norm();
|
||||
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_;
|
||||
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;
|
||||
*H /= pow(x2 + y2 + z2, 1.5);
|
||||
*H /= std::pow(x2 + y2 + z2, 1.5);
|
||||
}
|
||||
return normalized;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,18 +22,104 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
//#define GTSAM_USE_VECTOR3_POINTS
|
||||
#ifdef GTSAM_USE_VECTOR3_POINTS
|
||||
/**
|
||||
* A 3D point is just a Vector3 with some additional methods
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Point3 : public Vector3 {
|
||||
|
||||
public:
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor creates a zero-Point3
|
||||
Point3() { setZero(); }
|
||||
|
||||
/// Construct from x, y, and z coordinates
|
||||
Point3(double x, double y, double z): Vector3(x,y, z) {}
|
||||
|
||||
/// Construct from other vector
|
||||
template<typename Derived>
|
||||
inline Point3(const Eigen::MatrixBase<Derived>& v): Vector3(v) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Point3& p, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Point3 identity() { return Point3();}
|
||||
|
||||
/// @}
|
||||
/// @name Vector Space
|
||||
/// @{
|
||||
|
||||
/** distance between two points */
|
||||
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||
|
||||
/** Distance of the point from the origin, with Jacobian */
|
||||
double norm(OptionalJacobian<1,3> H = boost::none) const;
|
||||
|
||||
/** normalize, with optional Jacobian */
|
||||
Point3 normalize(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, //
|
||||
OptionalJacobian<3, 3> H_q = boost::none) const;
|
||||
|
||||
/** dot product @return this * q*/
|
||||
double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
|
||||
OptionalJacobian<1, 3> H_q = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
const Vector3& vector() const { return *this; }
|
||||
|
||||
/// @}
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
|
||||
}
|
||||
};
|
||||
#else
|
||||
/**
|
||||
* A 3D point
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Point3 {
|
||||
class GTSAM_EXPORT Point3 {
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -181,13 +267,15 @@ namespace gtsam {
|
|||
/// @}
|
||||
};
|
||||
|
||||
/// Syntactic sugar for multiplying coordinates by a scalar s*p
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
#endif
|
||||
|
||||
// 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> {};
|
||||
|
||||
|
|
|
|||
|
|
@ -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.vector() - t_.vector()));
|
||||
if (Dpose) {
|
||||
const double wx = q.x(), wy = q.y(), wz = q.z();
|
||||
(*Dpose) <<
|
||||
|
|
|
|||
|
|
@ -65,10 +65,12 @@ public:
|
|||
R_(R), t_(t) {
|
||||
}
|
||||
|
||||
/** Construct from R,t, where t \in vector3 */
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
/** Construct from R,t */
|
||||
Pose3(const Rot3& R, const Vector3& t) :
|
||||
R_(R), t_(t) {
|
||||
}
|
||||
#endif
|
||||
|
||||
/** Construct from Pose2 */
|
||||
explicit Pose3(const Pose2& pose2);
|
||||
|
|
|
|||
|
|
@ -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,6 +171,7 @@ namespace gtsam {
|
|||
return Rot3(q);
|
||||
}
|
||||
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
/**
|
||||
* Convert from axis/angle representation
|
||||
* @param axis is the rotation axis, unit length
|
||||
|
|
@ -181,9 +182,10 @@ namespace gtsam {
|
|||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
|
||||
#else
|
||||
return SO3::AxisAngle(axis,angle);
|
||||
return Rot3(SO3::AxisAngle(axis,angle));
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Convert from axis/angle representation
|
||||
|
|
@ -192,7 +194,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.vector(),angle));
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -315,7 +321,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,6 +358,14 @@ namespace gtsam {
|
|||
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
|
||||
OptionalJacobian<3,3> H2 = boost::none) const;
|
||||
|
||||
/// rotate point from rotated coordinate frame to world = R*p
|
||||
Point3 operator*(const Point3& p) const;
|
||||
|
||||
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
|
||||
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
|
||||
OptionalJacobian<3,3> H2=boost::none) const;
|
||||
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
/// operator* for Vector3
|
||||
inline Vector3 operator*(const Vector3& v) const {
|
||||
return rotate(Point3(v)).vector();
|
||||
|
|
@ -362,19 +376,12 @@ namespace gtsam {
|
|||
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;
|
||||
|
||||
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
|
||||
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();
|
||||
}
|
||||
#endif
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Unit3
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@
|
|||
#include <boost/random/variate_generator.hpp>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -252,11 +253,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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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) :
|
||||
|
|
|
|||
|
|
@ -43,10 +43,14 @@ class_<Point3>("Point3")
|
|||
.def("norm", &Point3::norm)
|
||||
.def("normalize", &Point3::normalize)
|
||||
.def("print", &Point3::print, print_overloads(args("s")))
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
.def("vector", &Point3::vector)
|
||||
.def("x", &Point3::x)
|
||||
.def("y", &Point3::y)
|
||||
.def("z", &Point3::z)
|
||||
#else
|
||||
.def("vector", &Point3::vector, return_value_policy<copy_const_reference>())
|
||||
#endif
|
||||
.def(self * other<double>())
|
||||
.def(other<double>() * self)
|
||||
.def(self + self)
|
||||
|
|
|
|||
|
|
@ -37,8 +37,11 @@ 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;
|
||||
#ifndef GTSAM_USE_VECTOR3_POINTS
|
||||
gtsam::Rot3 (*AxisAngle_2)(const Vector3&, double) = &Rot3::AxisAngle;
|
||||
#endif
|
||||
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 +61,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 +70,12 @@ void exportRot3(){
|
|||
.staticmethod("Logmap")
|
||||
.def("LogmapDerivative", &Rot3::LogmapDerivative)
|
||||
.staticmethod("LogmapDerivative")
|
||||
#ifdef GTSAM_USE_VECTOR3_POINTS
|
||||
.def("AxisAngle", AxisAngle_0)
|
||||
.def("AxisAngle", AxisAngle_1)
|
||||
#else
|
||||
.def("AxisAngle", AxisAngle_2)
|
||||
#endif
|
||||
.def("Rodrigues", Rodrigues_0)
|
||||
.def("Rodrigues", Rodrigues_1)
|
||||
.staticmethod("Rodrigues")
|
||||
|
|
|
|||
Loading…
Reference in New Issue