More GTSAM_EXPORT fixes. This allows gtsam_unstable to compile
parent
44faca1b1c
commit
ffb3043284
|
@ -37,7 +37,7 @@ namespace gtsam {
|
||||||
* @addtogroup geometry
|
* @addtogroup geometry
|
||||||
* \nosubgrouping
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
class GTSAM_EXPORT Point2 : public Vector2 {
|
class Point2 : public Vector2 {
|
||||||
private:
|
private:
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
@ -66,10 +66,10 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// print with optional string
|
/// print with optional string
|
||||||
void print(const std::string& s = "") const;
|
GTSAM_EXPORT void print(const std::string& s = "") const;
|
||||||
|
|
||||||
/// equals with an tolerance, prints out message if unequal
|
/// equals with an tolerance, prints out message if unequal
|
||||||
bool equals(const Point2& q, double tol = 1e-9) const;
|
GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Group
|
/// @name Group
|
||||||
|
@ -86,10 +86,10 @@ public:
|
||||||
Point2 unit() const { return *this/norm(); }
|
Point2 unit() const { return *this/norm(); }
|
||||||
|
|
||||||
/** norm of point, with derivative */
|
/** norm of point, with derivative */
|
||||||
double norm(OptionalJacobian<1,2> H = boost::none) const;
|
GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const;
|
||||||
|
|
||||||
/** distance between two points */
|
/** distance between two points */
|
||||||
double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
|
GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
|
||||||
OptionalJacobian<1,2> H2 = boost::none) const;
|
OptionalJacobian<1,2> H2 = boost::none) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
@ -124,9 +124,9 @@ public:
|
||||||
static Vector2 Logmap(const Point2& p) { return p;}
|
static Vector2 Logmap(const Point2& p) { return p;}
|
||||||
static Point2 Expmap(const Vector2& v) { return Point2(v);}
|
static Point2 Expmap(const Vector2& v) { return Point2(v);}
|
||||||
inline double dist(const Point2& p2) const {return distance(p2);}
|
inline double dist(const Point2& p2) const {return distance(p2);}
|
||||||
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
|
GTSAM_EXPORT static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
|
||||||
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
|
GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
|
||||||
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
|
GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
|
||||||
/// @}
|
/// @}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,7 @@ namespace gtsam {
|
||||||
* @addtogroup geometry
|
* @addtogroup geometry
|
||||||
* \nosubgrouping
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
class GTSAM_EXPORT Point3 : public Vector3 {
|
class Point3 : public Vector3 {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
@ -63,10 +63,10 @@ class GTSAM_EXPORT Point3 : public Vector3 {
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/** print with optional string */
|
/** print with optional string */
|
||||||
void print(const std::string& s = "") const;
|
GTSAM_EXPORT void print(const std::string& s = "") const;
|
||||||
|
|
||||||
/** equals with an tolerance */
|
/** equals with an tolerance */
|
||||||
bool equals(const Point3& p, double tol = 1e-9) const;
|
GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Group
|
/// @name Group
|
||||||
|
@ -80,21 +80,21 @@ class GTSAM_EXPORT Point3 : public Vector3 {
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/** distance between two points */
|
/** distance between two points */
|
||||||
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
|
GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
|
||||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||||
|
|
||||||
/** Distance of the point from the origin, with Jacobian */
|
/** Distance of the point from the origin, with Jacobian */
|
||||||
double norm(OptionalJacobian<1,3> H = boost::none) const;
|
GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const;
|
||||||
|
|
||||||
/** normalize, with optional Jacobian */
|
/** normalize, with optional Jacobian */
|
||||||
Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
|
GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
|
||||||
|
|
||||||
/** cross product @return this x q */
|
/** cross product @return this x q */
|
||||||
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
|
GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
|
||||||
OptionalJacobian<3, 3> H_q = boost::none) const;
|
OptionalJacobian<3, 3> H_q = boost::none) const;
|
||||||
|
|
||||||
/** dot product @return this * q*/
|
/** dot product @return this * q*/
|
||||||
double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
|
GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
|
||||||
OptionalJacobian<1, 3> H_q = boost::none) const;
|
OptionalJacobian<1, 3> H_q = boost::none) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
@ -130,9 +130,9 @@ class GTSAM_EXPORT Point3 : public Vector3 {
|
||||||
static Point3 Expmap(const Vector3& v) { return Point3(v);}
|
static Point3 Expmap(const Vector3& v) { return Point3(v);}
|
||||||
inline double dist(const Point3& q) const { return (q - *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 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
|
||||||
Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
|
GTSAM_EXPORT Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
|
||||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||||
Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
|
GTSAM_EXPORT Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
|
||||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||||
/// @}
|
/// @}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -33,7 +33,7 @@ namespace gtsam {
|
||||||
* @addtogroup geometry
|
* @addtogroup geometry
|
||||||
* \nosubgrouping
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
|
class Pose2: public LieGroup<Pose2, 3> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
@ -97,10 +97,10 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/** print with optional string */
|
/** print with optional string */
|
||||||
void print(const std::string& s = "") const;
|
GTSAM_EXPORT void print(const std::string& s = "") const;
|
||||||
|
|
||||||
/** assert equality up to a tolerance */
|
/** assert equality up to a tolerance */
|
||||||
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Group
|
/// @name Group
|
||||||
|
@ -110,7 +110,7 @@ public:
|
||||||
inline static Pose2 identity() { return Pose2(); }
|
inline static Pose2 identity() { return Pose2(); }
|
||||||
|
|
||||||
/// inverse
|
/// inverse
|
||||||
Pose2 inverse() const;
|
GTSAM_EXPORT Pose2 inverse() const;
|
||||||
|
|
||||||
/// compose syntactic sugar
|
/// compose syntactic sugar
|
||||||
inline Pose2 operator*(const Pose2& p2) const {
|
inline Pose2 operator*(const Pose2& p2) const {
|
||||||
|
@ -122,16 +122,16 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
|
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
|
||||||
static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
|
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
|
||||||
|
|
||||||
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
|
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
|
||||||
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
|
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate Adjoint map
|
* Calculate Adjoint map
|
||||||
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
|
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
|
||||||
*/
|
*/
|
||||||
Matrix3 AdjointMap() const;
|
GTSAM_EXPORT Matrix3 AdjointMap() const;
|
||||||
|
|
||||||
/// Apply AdjointMap to twist xi
|
/// Apply AdjointMap to twist xi
|
||||||
inline Vector3 Adjoint(const Vector3& xi) const {
|
inline Vector3 Adjoint(const Vector3& xi) const {
|
||||||
|
@ -141,7 +141,7 @@ public:
|
||||||
/**
|
/**
|
||||||
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
|
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
|
||||||
*/
|
*/
|
||||||
static Matrix3 adjointMap(const Vector3& v);
|
GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
|
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
|
||||||
|
@ -177,15 +177,15 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Derivative of Expmap
|
/// Derivative of Expmap
|
||||||
static Matrix3 ExpmapDerivative(const Vector3& v);
|
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
|
||||||
|
|
||||||
/// Derivative of Logmap
|
/// Derivative of Logmap
|
||||||
static Matrix3 LogmapDerivative(const Pose2& v);
|
GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
|
||||||
|
|
||||||
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
|
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
|
||||||
struct ChartAtOrigin {
|
struct ChartAtOrigin {
|
||||||
static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
|
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
|
||||||
static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
|
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
|
||||||
};
|
};
|
||||||
|
|
||||||
using LieGroup<Pose2, 3>::inverse; // version with derivative
|
using LieGroup<Pose2, 3>::inverse; // version with derivative
|
||||||
|
@ -195,12 +195,12 @@ public:
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/** Return point coordinates in pose coordinate frame */
|
/** Return point coordinates in pose coordinate frame */
|
||||||
Point2 transformTo(const Point2& point,
|
GTSAM_EXPORT Point2 transformTo(const Point2& point,
|
||||||
OptionalJacobian<2, 3> Dpose = boost::none,
|
OptionalJacobian<2, 3> Dpose = boost::none,
|
||||||
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
||||||
|
|
||||||
/** Return point coordinates in global frame */
|
/** Return point coordinates in global frame */
|
||||||
Point2 transformFrom(const Point2& point,
|
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
|
||||||
OptionalJacobian<2, 3> Dpose = boost::none,
|
OptionalJacobian<2, 3> Dpose = boost::none,
|
||||||
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
||||||
|
|
||||||
|
@ -233,14 +233,14 @@ public:
|
||||||
inline const Rot2& rotation() const { return r_; }
|
inline const Rot2& rotation() const { return r_; }
|
||||||
|
|
||||||
//// return transformation matrix
|
//// return transformation matrix
|
||||||
Matrix3 matrix() const;
|
GTSAM_EXPORT Matrix3 matrix() const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate bearing to a landmark
|
* Calculate bearing to a landmark
|
||||||
* @param point 2D location of landmark
|
* @param point 2D location of landmark
|
||||||
* @return 2D rotation \f$ \in SO(2) \f$
|
* @return 2D rotation \f$ \in SO(2) \f$
|
||||||
*/
|
*/
|
||||||
Rot2 bearing(const Point2& point,
|
GTSAM_EXPORT Rot2 bearing(const Point2& point,
|
||||||
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
|
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -248,7 +248,7 @@ public:
|
||||||
* @param point SO(2) location of other pose
|
* @param point SO(2) location of other pose
|
||||||
* @return 2D rotation \f$ \in SO(2) \f$
|
* @return 2D rotation \f$ \in SO(2) \f$
|
||||||
*/
|
*/
|
||||||
Rot2 bearing(const Pose2& pose,
|
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
|
||||||
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
|
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -256,7 +256,7 @@ public:
|
||||||
* @param point 2D location of landmark
|
* @param point 2D location of landmark
|
||||||
* @return range (double)
|
* @return range (double)
|
||||||
*/
|
*/
|
||||||
double range(const Point2& point,
|
GTSAM_EXPORT double range(const Point2& point,
|
||||||
OptionalJacobian<1, 3> H1=boost::none,
|
OptionalJacobian<1, 3> H1=boost::none,
|
||||||
OptionalJacobian<1, 2> H2=boost::none) const;
|
OptionalJacobian<1, 2> H2=boost::none) const;
|
||||||
|
|
||||||
|
@ -265,7 +265,7 @@ public:
|
||||||
* @param point 2D location of other pose
|
* @param point 2D location of other pose
|
||||||
* @return range (double)
|
* @return range (double)
|
||||||
*/
|
*/
|
||||||
double range(const Pose2& point,
|
GTSAM_EXPORT double range(const Pose2& point,
|
||||||
OptionalJacobian<1, 3> H1=boost::none,
|
OptionalJacobian<1, 3> H1=boost::none,
|
||||||
OptionalJacobian<1, 3> H2=boost::none) const;
|
OptionalJacobian<1, 3> H2=boost::none) const;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue