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

Some additional derivatives (for cvross) and operators

release/4.3a0
Frank 2015-10-19 14:44:00 -07:00
parent 4c22355f56
commit 06520a3b1d
2 changed files with 57 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/geometry/Point3.cpp
View File

@ -58,6 +58,22 @@ Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
*H1 = *H1 *(1. / d);
}
if (H2) {
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
*H2 = *H2 *(1. / d);
}
return d;
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
@ -75,13 +91,27 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
if (H_p) {
*H_p << skewSymmetric(-q.vector());
}
if (H_q) {
*H_q << skewSymmetric(vector());
}
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
if (H_p) {
*H_p << q.vector().transpose();
}
if (H_q) {
*H_q << vector().transpose();
}
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
}
@ -116,6 +146,12 @@ ostream &operator<<(ostream &os, const Point3& p) {
return os;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
os << p.first << " <-> " << p.second;
return os;
}
/* ************************************************************************* */
} // namespace gtsam

39
gtsam/geometry/Point3.h
View File

@ -37,8 +37,8 @@ namespace gtsam {
private:
double x_, y_, z_;
double x_, y_, z_;
public:
enum { dimension = 3 };
@ -56,7 +56,7 @@ namespace gtsam {
/// @{
/// Construct from 3-element vector
Point3(const Vector3& v) {
explicit Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
@ -82,6 +82,11 @@ namespace gtsam {
/// inverse
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// add vector on right
inline Point3 operator +(const Vector3& v) const {
return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
}
/// add
Point3 operator + (const Point3& q) const;
@ -99,20 +104,8 @@ namespace gtsam {
Point3 operator / (double s) const;
/** distance between two points */
inline double distance(const Point3& p2,
OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
*H1 = *H1 *(1./d);
}
if (H2) {
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
*H2 << *H2 *(1./d);
}
return d;
}
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point3& p2) const {
@ -126,17 +119,19 @@ namespace gtsam {
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
OptionalJacobian<3, 3> H_q = boost::none) const;
/** dot product @return this * q*/
double dot(const Point3 &q) const;
double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
OptionalJacobian<1, 3> H_q = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// equality
bool operator ==(const Point3& q) const;
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector3 vector() const { return Vector3(x_,y_,z_); }
@ -192,6 +187,10 @@ namespace gtsam {
/// @}
};
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}