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

Switched geometry to use primarily member functions for calculations with optional derivatives, so there are no more optional derivative functions. Also split Vector specializations for Lie into gtsam/base/LieVector.h which will later change into a real wrapper function. Specialized numericalDerivative to allow for functions to return doubles. Projects ISAM2, MastSLAM updated.

release/4.3a0
Alex Cunningham 2010-08-22 21:45:53 +00:00
parent 9604cd4507
commit 458cc52fff
41 changed files with 881 additions and 883 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
base/Lie.h
View File

@ -132,33 +132,6 @@ namespace gtsam {
return obj1.equals(obj2);
}
// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
// double,+ group operations
inline double compose(double p1,double p2) { return p1+p2;}
inline double inverse(double p) { return -p;}
inline double between(double p1,double p2) { return p2-p1;}
// double,+ is a trivial Lie group
template<> inline double expmap(const Vector& d) { return d(0);}
template<> inline double expmap(const double& p,const Vector& d) { return p+d(0);}
inline Vector logmap(const double& p) { return repeat(1,p);}
inline Vector logmap(const double& p1,const double& p2) { return Vector_(1,p2-p1);}
// Global functions needed for double
inline size_t dim(const double& v) { return 1; }
// Vector group operations
inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
inline Vector inverse(const Vector& p) { return -p;}
inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
// Vector is a trivial Lie group
template<> inline Vector expmap(const Vector& d) { return d;}
template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
inline Vector logmap(const Vector& p) { return p;}
inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)

27
base/LieVector.h Normal file
View File

@ -0,0 +1,27 @@
/**
* @file LieVector.h
* @brief A wrapper around vector providing Lie compatibility
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/Lie.h>
namespace gtsam {
/** temporarily using the old system */
// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
// Vector group operations
inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
inline Vector inverse(const Vector& p) { return -p;}
inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
// Vector is a trivial Lie group
template<> inline Vector expmap(const Vector& d) { return d;}
template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
template<> inline Vector logmap(const Vector& p) { return p;}
template<> inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
} // \namespace gtsam

2
base/Makefile.am
View File

@ -26,7 +26,7 @@ endif
headers += Testable.h TestableAssertions.h numericalDerivative.h
# Lie Groups
headers += Lie.h Lie-inl.h
headers += Lie.h Lie-inl.h LieVector.h
# Data structures
headers += BTree.h DSF.h

161
base/numericalDerivative.h
View File

@ -94,6 +94,28 @@ namespace gtsam {
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
}
/** pseudo-template specialization for double Y values */
template<class X>
Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
double hx = h(x);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1, h(expmap(x,d))-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1, h(expmap(x,d))-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X>
Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
return numericalDerivative11<X>(boost::bind(h, _1), x, delta);
}
/**
* Compute numerical derivative in argument 1 of binary function
* @param h binary function yielding m-vector
@ -121,11 +143,35 @@ namespace gtsam {
}
template<class Y, class X1, class X2>
Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
/** pseudo-partial template specialization for double return values */
template<class X1, class X2>
Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
double hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x1);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1, h(expmap(x1,d),x2)-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1, h(expmap(x1,d),x2)-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X1, class X2>
inline Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
/**
* Compute numerical derivative in argument 2 of binary function
* @param h binary function yielding m-vector
@ -154,12 +200,34 @@ namespace gtsam {
return H;
}
template<class Y, class X1, class X2>
Matrix numericalDerivative22
(Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
inline Matrix numericalDerivative22
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
/** pseudo-specialization for double Y values */
template<class X1, class X2>
Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
double hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x2);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1,h(x1,expmap(x2,d))-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1,expmap(x2,d))-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X1, class X2>
inline Matrix numericalDerivative22 (double (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
/**
* Compute numerical derivative in argument 1 of ternary function
* @param h ternary function yielding m-vector
@ -189,7 +257,7 @@ namespace gtsam {
return H;
}
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31
inline Matrix numericalDerivative31
(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
@ -215,7 +283,7 @@ namespace gtsam {
return H;
}
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative32
inline Matrix numericalDerivative32
(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
@ -241,9 +309,88 @@ namespace gtsam {
return H;
}
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative33
inline Matrix numericalDerivative33
(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
}
/**
* specializations for double outputs
*/
template<class X1, class X2, class X3>
Matrix numericalDerivative31
(boost::function<double(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
double hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x1);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative31
(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative31<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
}
// arg 2
template<class X1, class X2, class X3>
Matrix numericalDerivative32
(boost::function<double(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
double hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x2);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative32
(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative32<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
}
// arg 3
template<class X1, class X2, class X3>
Matrix numericalDerivative33
(boost::function<double(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
double hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = 1, n = dim(x3);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative33
(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative33<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
}
}

23
geometry/Cal3_S2.cpp
View File

@ -42,23 +42,12 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
return K.uncalibrate(p);
}
/* ************************************************************************* */
Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p) {
return Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,
1.0);
}
/* ************************************************************************* */
Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p) {
return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
Point2 Cal3_S2::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,1.0);
if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */

15
geometry/Cal3_S2.h
View File

@ -85,11 +85,11 @@ namespace gtsam {
/**
* convert intrinsic coordinates xy to image coordinates uv
* with optional derivatives
*/
Point2 uncalibrate(const Point2& p) const {
const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
@ -100,7 +100,6 @@ namespace gtsam {
}
/** friends */
friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d);
private:
@ -132,10 +131,4 @@ namespace gtsam {
cal.s_ + d(2), cal.u0_ + d(3), cal.v0_ + d(4));
}
/**
* convert intrinsic coordinates xy to image coordinates uv
*/
Point2 uncalibrate(const Cal3_S2& K, const Point2& p);
Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p);
Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
}

92
geometry/CalibratedCamera.cpp
View File

@ -10,27 +10,6 @@
namespace gtsam {
/* ************************************************************************* */
// Auxiliary functions
/* ************************************************************************* */
Point2 project_to_camera(const Point3& P) {
return Point2(P.x() / P.z(), P.y() / P.z());
}
Matrix Dproject_to_camera1(const Point3& P) {
double d = 1.0 / P.z(), d2 = d * d;
return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
Point3 backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
/* ************************************************************************* */
// Methods
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) {
}
@ -39,6 +18,18 @@ namespace gtsam {
CalibratedCamera::~CalibratedCamera() {}
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
double st = sin(pose2.theta()), ct = cos(pose2.theta());
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
@ -48,66 +39,29 @@ namespace gtsam {
return CalibratedCamera(pose3);
}
Point2 CalibratedCamera::project(const Point3 & P) const {
Point3 cameraPoint = Pose3::transform_to(pose_, P);
Point2 intrinsic = project_to_camera(cameraPoint);
return intrinsic;
}
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
Point2 project(const CalibratedCamera& camera, const Point3& point) {
return camera.project(point);
}
/* ************************************************************************* */
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {
const Pose3& pose = camera.pose();
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> D_intrinsic_point) const {
const Pose3& pose = pose_;
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = Pose3::transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -d*r1.x()+r3.x()*Xd2, -d*r1.y()+r3.y()*Xd2, -d*r1.z()+r3.z()*Xd2,
d*Z+Y*Yd2, -X*Yd2, -d*X, -d*r2.x()+r3.x()*Yd2, -d*r2.y()+r3.y()*Yd2, -d*r2.z()+r3.z()*Yd2);
}
Point3 q = pose.transform_to(point);
/* ************************************************************************* */
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = Pose3::transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,3,
d*r1.x()-r3.x()*Xd2, d*r1.y()-r3.y()*Xd2, d*r1.z()-r3.z()*Xd2,
d*r2.x()-r3.x()*Yd2, d*r2.y()-r3.y()*Yd2, d*r2.z()-r3.z()*Yd2);
}
/* ************************************************************************* */
Point2 Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = Pose3::transform_to(pose, point);
if (D_intrinsic_pose || D_intrinsic_point) {
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
D_intrinsic_pose = Matrix_(2,6,
if (D_intrinsic_pose)
*D_intrinsic_pose = Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
D_intrinsic_point = Matrix_(2,3,
if (D_intrinsic_point)
*D_intrinsic_point = Matrix_(2,3,
dp11, dp12, dp13,
dp21, dp22, dp23);
}
return project_to_camera(q);
}
/* ************************************************************************* */
}

40
geometry/CalibratedCamera.h
View File

@ -15,22 +15,6 @@ namespace gtsam {
class Point2;
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
*/
Point2 project_to_camera(const Point3& cameraPoint);
/**
* Derivative of project_to_camera
*/
Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
Point3 backproject_from_camera(const Point2& p, const double scale);
/**
* A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient
@ -67,9 +51,6 @@ namespace gtsam {
*/
static CalibratedCamera level(const Pose2& pose2, double height);
Point2 project(const Point3& P) const;
};
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
@ -81,21 +62,24 @@ namespace gtsam {
* @param point a 3D point to be projected
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const CalibratedCamera& camera, const Point3& point);
Point2 project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose = boost::none,
boost::optional<Matrix&> D_intrinsic_point = boost::none) const;
/**
* Derivatives of project, same paramaters as project
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative
*/
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point);
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point);
static Point2 project_to_camera(const Point3& cameraPoint,
boost::optional<Matrix&> H1 = boost::none);
/**
* super-duper combined evaluation + derivatives
* saves a lot of time because a lot of computation is shared
* @return project(camera,point)
* backproject a 2-dimensional point to a 3-dimension point
*/
Point2 Dproject_pose_point(const CalibratedCamera& camera,
const Point3& point, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
static Point3 backproject_from_camera(const Point2& p, const double scale);
};
}
#endif /* CalibratedCAMERA_H_ */

43
geometry/Point3.cpp
View File

@ -48,43 +48,40 @@ namespace gtsam {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &p, const Point3 &q) {
return p+q;
}
// Point3 Point3::add(const Point3 &q) const {
// return *this + q;
// }
/* ************************************************************************* */
Point3 Point3::add(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
return add(p,q);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &p, const Point3 &q) {
return p+q;
}
// Point3 Point3::sub(const Point3 &q) const {
// return *this - q;
// }
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
return sub(p,q);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &p, const Point3 &q)
{
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_ );
Point3 Point3::cross(const Point3 &q) const {
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 &p, const Point3 &q)
{
return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
}
/* ************************************************************************* */
double Point3::norm(const Point3 &p)
{
return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
}
/* ************************************************************************* */

31
geometry/Point3.h
View File

@ -84,29 +84,22 @@ namespace gtsam {
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
}
/** add two points, add(p,q) is same as p+q */
static Point3 add (const Point3 &p, const Point3 &q);
static Point3 add (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** subtract two points, sub(p,q) is same as p-q */
static Point3 sub (const Point3 &p, const Point3 &q);
static Point3 sub (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** cross product */
static Point3 cross(const Point3 &p, const Point3 &q);
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;
/** dot product @return this * q*/
double dot(const Point3 &q) const;
/** dot product */
static double dot(const Point3 &p, const Point3 &q);
/** dot product */
static double norm(const Point3 &p);
// /** friends */
// friend Point3 cross(const Point3 &p1, const Point3 &p2);
// friend double dot(const Point3 &p1, const Point3 &p2);
// friend double norm(const Point3 &p1);
double norm() const;
private:
/** Serialization function */

63
geometry/Pose2.cpp
View File

@ -83,10 +83,8 @@ namespace gtsam {
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix AdjointMap(const Pose2& p) {
const Rot2 R = p.r();
const Point2 t = p.t();
double c = R.c(), s = R.s(), x = t.x(), y = t.y();
Matrix Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return Matrix_(3,3,
c, -s, y,
s, c, -x,
@ -96,55 +94,50 @@ namespace gtsam {
/* ************************************************************************* */
Pose2 Pose2::inverse() const {
const Rot2& R = r_;
const Point2& t = t_;
return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y())));
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1) {
if (H1) *H1 = -AdjointMap(pose);
if (H1) *H1 = -pose.AdjointMap();
return pose.inverse();
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Pose2& pose, const Point2& point, boost::optional<
Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& R = pose.r();
Point2 d = point - pose.t();
Point2 q = R.unrotate(d);
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = Matrix_(2, 3,
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = R.transpose();
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
// TODO: inline and reuse?
if(H1) *H1 = AdjointMap(inverse(p2));
if(H1) *H1 = inverse(p2).AdjointMap();
if(H2) *H2 = I3;
return p1*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Pose2& pose, const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& rot = pose.r();
const Point2 q = rot * p;
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = rot.matrix();
const Matrix R = r_.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + pose.t();
return q + t_;
}
/* ************************************************************************* */
@ -180,15 +173,10 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot2 bearing(const Pose2& pose, const Point2& point) {
Point2 d = Pose2::transform_to(pose, point);
return Rot2::relativeBearing(d);
}
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return bearing(pose, point);
Point2 d = Pose2::transform_to(pose, point, H1, H2);
Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2);
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
@ -197,15 +185,10 @@ namespace gtsam {
}
/* ************************************************************************* */
double range(const Pose2& pose, const Point2& point) {
Point2 d = Pose2::transform_to(pose, point);
return d.norm();
}
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return range(pose, point);
Point2 d = Pose2::transform_to(pose, point, H1, H2);
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point2 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * (*H1);

103
geometry/Pose2.h
View File

@ -63,6 +63,7 @@ namespace gtsam {
/** assert equality up to a tolerance */
bool equals(const Pose2& pose, double tol = 1e-9) const;
/** compose syntactic sugar */
inline Pose2 operator*(const Pose2& p2) const {
return Pose2(r_*p2.r(), t_ + r_*p2.t());
}
@ -81,6 +82,9 @@ namespace gtsam {
/** compose with another pose */
inline Pose2 compose(const Pose2& p) const { return *this * p; }
/** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) { return transform_from(point);}
/**
* Exponential map from se(2) to SE(2)
*/
@ -97,18 +101,53 @@ namespace gtsam {
/**
* Return point coordinates in pose coordinate frame
*/
static inline Point2 transform_to(const Pose2& pose, const Point2& point)
{ return Rot2::unrotate(pose.r(), point - pose.t());}
static Point2 transform_to(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Return point coordinates in global frame
*/
static inline Point2 transform_from(const Pose2& pose, const Point2& point)
{ return Rot2::rotate(pose.r(), point) + pose.t();}
static Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate bearing to a landmark
* @param point 2D location of landmark
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to a landmark
* @param point 2D location of landmark
* @return range (double)
*/
double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
return AdjointMap()*xi;
}
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
* omega is angular velocity
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return Matrix_(3,3,
0.,-w, vx,
w, 0., vy,
0., 0., 0.);
}
/** get functions for x, y, theta */
inline double x() const { return t_.x(); }
@ -128,30 +167,10 @@ namespace gtsam {
}
}; // Pose2
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap(const Pose2& p);
inline Vector Adjoint(const Pose2& p, const Vector& xi) { return AdjointMap(p)*xi;}
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
* omega is angular velocity
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
inline Matrix wedge(double vx, double vy, double w) {
return Matrix_(3,3,
0.,-w, vx,
w, 0., vy,
0., 0., 0.);
}
/** specialization for pose2 wedge function (generic template in Lie.h) */
template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
return wedge(xi(0),xi(1),xi(2));
return Pose2::wedge(xi(0),xi(1),xi(2));
}
/**
@ -166,35 +185,11 @@ namespace gtsam {
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none);
/** syntactic sugar for transform_from */
inline Point2 operator*(const Pose2& pose, const Point2& point)
{ return Pose2::transform_from(pose, point);}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Calculate bearing to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Pose2& pose, const Point2& point);
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Calculate range to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return range (double)
*/
double range(const Pose2& pose, const Point2& point);
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
} // namespace gtsam

51
geometry/Pose3.cpp
View File

@ -21,9 +21,9 @@ namespace gtsam {
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix AdjointMap(const Pose3& p) {
const Matrix R = p.rotation().matrix();
const Vector t = p.translation().vector();
Matrix Pose3::AdjointMap() const {
const Matrix R = R_.matrix();
const Vector t = t_.vector();
Matrix A = skewSymmetric(t)*R;
Matrix DR = collect(2, &R, &Z3);
Matrix Dt = collect(2, &A, &R);
@ -134,55 +134,56 @@ namespace gtsam {
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_));
Point3 t = Pose3::transform_to(pose, t_);
Point3 t = pose.transform_to(t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
return pose.rotation() * p + pose.translation();
}
// Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
// return pose.rotation() * p + pose.translation();
// }
/* ************************************************************************* */
Point3 Pose3::transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point3 Pose3::transform_from(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
const Matrix R = pose.rotation().matrix();
const Matrix R = R_.matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 = collect(2,&DR,&R);
#else
Matrix DR;
Rot3::rotate(pose.rotation(), p, DR, boost::none);
R_.rotate(p, DR, boost::none);
*H1 = collect(2,&DR,&I3);
#endif
}
if (H2) *H2 = pose.rotation().matrix();
return Pose3::transform_from(pose, p);
if (H2) *H2 = R_.matrix();
return R_ * p + t_;
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
Point3 sub = p - pose.translation();
return Rot3::unrotate(pose.rotation(), sub);
}
// Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
// Point3 sub = p - pose.translation();
// return pose.rotation().unrotate(sub);
// }
/* ************************************************************************* */
Point3 Pose3::transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point3 Pose3::transform_to(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point3 result = R_.unrotate(p - t_);
if (H1) { // *H1 = Dtransform_to1(pose, p);
Point3 q = transform_to(pose,p);
const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
#ifdef CORRECT_POSE3_EXMAP
*H1 = collect(2, &DR, &_I3);
#else
Matrix DT = - pose.rotation().transpose(); // negative because of sub
Matrix DT = - R_.transpose(); // negative because of sub
*H1 = collect(2,&DR,&DT);
#endif
}
if (H2) *H2 = pose.rotation().transpose();
return Pose3::transform_to(pose, p);
if (H2) *H2 = R_.transpose();
return result;
}
/* ************************************************************************* */
@ -197,7 +198,7 @@ namespace gtsam {
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt;
Pose3::transform_from(p1,t2, Dt, boost::none);
p1.transform_from(t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt);
#endif
}
@ -226,7 +227,7 @@ namespace gtsam {
const Point3& t = p.translation();
Matrix Rt = R.transpose();
Matrix DR_R1 = -R.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(Rot3::unrotate(R,t).vector()), Dt_t1 = -Rt;
Matrix Dt_R1 = -skewSymmetric(R.unrotate(t).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt);

62
geometry/Pose3.h
View File

@ -87,14 +87,15 @@ namespace gtsam {
inline Pose3 compose(const Pose3& t) const { return *this * t; }
/** receives the point in Pose coordinates and transforms it to world coordinates */
static Point3 transform_from(const Pose3& pose, const Point3& p);
static Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** syntactic sugar for transform */
inline Point3 operator*(const Point3& p) { return transform_from(p); }
/** receives the point in world coordinates and transforms it to Pose coordinates */
static Point3 transform_to(const Pose3& pose, const Point3& p);
static Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a pose with a translation and
* rotation (in canonical coordinates). */
@ -104,6 +105,28 @@ namespace gtsam {
* coordinates of a pose. */
static Vector Logmap(const Pose3& p);
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return Matrix_(4,4,
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.);
}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -120,9 +143,6 @@ namespace gtsam {
/** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T1, const Pose3& T2);
/** syntactic sugar for transform */
inline Point3 operator*(const Pose3& pose, const Point3& p) { return Pose3::transform_from(pose, p); }
/**
* Derivatives of compose
*/
@ -140,21 +160,6 @@ namespace gtsam {
Pose3 between(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return Matrix_(4,4,
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.);
}
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
@ -164,14 +169,7 @@ namespace gtsam {
*/
template <>
inline Matrix wedge<Pose3>(const Vector& xi) {
return wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap(const Pose3& p);
inline Vector Adjoint(const Pose3& p, const Vector& xi) {return AdjointMap(p)*xi; }
} // namespace gtsam

96
geometry/Rot2.cpp
View File

@ -12,37 +12,37 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot2);
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot2);
/* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) {
/* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) {
if (fabs(c * c + s * s - 1.0) > 1e-9) {
double norm_cs = sqrt(c*c + s*s);
c = c/norm_cs;
s = s/norm_cs;
}
return Rot2(c, s);
}
}
/* ************************************************************************* */
Rot2 Rot2::atan2(double y, double x) {
/* ************************************************************************* */
Rot2 Rot2::atan2(double y, double x) {
Rot2 R(x, y);
return R.normalize();
}
}
/* ************************************************************************* */
void Rot2::print(const string& s) const {
/* ************************************************************************* */
void Rot2::print(const string& s) const {
cout << s << ":" << theta() << endl;
}
}
/* ************************************************************************* */
bool Rot2::equals(const Rot2& R, double tol) const {
/* ************************************************************************* */
bool Rot2::equals(const Rot2& R, double tol) const {
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
}
}
/* ************************************************************************* */
Rot2& Rot2::normalize() {
/* ************************************************************************* */
Rot2& Rot2::normalize() {
double scale = c_*c_ + s_*s_;
if(fabs(scale-1.0)>1e-9) {
scale = pow(scale, -0.5);
@ -50,60 +50,44 @@ namespace gtsam {
s_ *= scale;
}
return *this;
}
}
/* ************************************************************************* */
Matrix Rot2::matrix() const {
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return Matrix_(2, 2, c_, -s_, s_, c_);
}
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return Matrix_(2, 2, c_, s_, -s_, c_);
}
}
/* ************************************************************************* */
Point2 Rot2::rotate(const Point2& p) const {
return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
}
/* ************************************************************************* */
Point2 Rot2::unrotate(const Point2& p) const {
return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
}
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
Point2 q = R * p;
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
if (H2) *H2 = R.matrix();
if (H2) *H2 = matrix();
return q;
}
}
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::unrotate(const Rot2 & R, const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point2 q = R.unrotate(p);
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); // R_{pi/2}q
if (H2) *H2 = R.transpose();
if (H2) *H2 = transpose();
return q;
}
}
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d) {
double n = d.norm();
return Rot2::fromCosSin(d.x() / n, d.y() / n);
}
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
return Rot2::fromCosSin(x / n, y / n);
}
}
/* ************************************************************************* */

31
geometry/Rot2.h
View File

@ -57,23 +57,16 @@ namespace gtsam {
/** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
static Rot2 fromCosSin(double c, double s);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
/**
* Named constructor
* Named constructor with derivative
* Calculate relative bearing to a landmark in local coordinate frame
* @param point 2D location of landmark
* @param H optional reference for Jacobian
* @return 2D rotation \in SO(2)
*/
static Rot2 relativeBearing(const Point2& d);
/**
* Named constructor with derivative
* Calculate relative bearing and optional derivative
*/
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H);
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H=boost::none);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
static Rot2 atan2(double y, double x);
/** return angle (RADIANS) */
@ -133,25 +126,22 @@ namespace gtsam {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** rotate from world to rotated = R*p */
Point2 rotate(const Point2& p) const;
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
static Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
/** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const;
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {return rotate(p);}
/**
* rotate point from world to rotated
* frame = R'*p
*/
static Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
private:
/** Serialization function */
@ -164,9 +154,6 @@ namespace gtsam {
}; // Rot2
/** syntactic sugar for rotate */
inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);}
} // gtsam
#endif /* ROT2_H_ */

26
geometry/Rot3.cpp
View File

@ -166,24 +166,24 @@ namespace gtsam {
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R.matrix();
return rotate(R,p);
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = matrix();
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}
/* ************************************************************************* */
Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
const Matrix Rt(R.transpose());
return Rt*p.vector(); // q = Rt*p
}
// /* ************************************************************************* */
// Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
// const Matrix Rt(R.transpose());
// return Rt*p.vector(); // q = Rt*p
// }
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
const Matrix Rt(R.transpose());
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;

16
geometry/Rot3.h
View File

@ -173,25 +173,25 @@ namespace gtsam {
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p) const
{return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
// Point3 rotate(const Point3& p) const
// {return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
static Point3 rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
// static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
static Point3 unrotate(const Rot3& R, const Point3& p);
static Point3 unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
// static Point3 unrotate(const Rot3& R, const Point3& p);
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */

77
geometry/SimpleCamera.cpp
View File

@ -27,73 +27,40 @@ namespace gtsam {
}
pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const {
Point3 cameraPoint = Pose3::transform_to(calibrated_.pose(), P);
Point2 intrinsic = project_to_camera(cameraPoint);
Point2 projection = uncalibrate(K_, intrinsic);
Point3 cameraPoint = calibrated_.pose().transform_to(P);
Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint);
Point2 projection = K_.uncalibrate(intrinsic);
return pair<Point2, bool>(projection, cameraPoint.z() > 0);
}
Point2 SimpleCamera::project(const Point3 & P) const {
pair<Point2, bool> projected = projectSafe(P);
return projected.first;
Point2 SimpleCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 intrinsic = calibrated_.project(point, H1, H2);
if (!H1 && !H2)
return K_.uncalibrate(intrinsic);
Matrix D_projection_intrinsic;
Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
if (H1) {
*H1 = D_projection_intrinsic * (*H1);
}
if (H2) {
*H2 = D_projection_intrinsic * (*H2);
}
return projection;
}
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
Point3 cameraPoint = backproject_from_camera(intrinsic, scale);
return Pose3::transform_from(calibrated_.pose(), cameraPoint);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
return calibrated_.pose().transform_from(cameraPoint);
}
SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
return SimpleCamera(K, CalibratedCamera::level(pose2, height));
}
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
pair<Point2, bool> projectSafe(const SimpleCamera& camera, const Point3& point) {
return camera.projectSafe(point);
}
Point2 project(const SimpleCamera& camera, const Point3& point) {
return camera.project(point);
}
/* ************************************************************************* */
Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point) {
Point2 intrinsic = project(camera.calibrated_, point);
Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
return D_projection_pose;
}
/* ************************************************************************* */
Matrix Dproject_point(const SimpleCamera& camera, const Point3& point) {
Point2 intrinsic = project(camera.calibrated_, point);
Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
return D_projection_point;
}
/* ************************************************************************* */
Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point) {
Point2 intrinsic = project(camera.calibrated_, point);
Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
Point2 projection = uncalibrate(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
D_projection_point = D_projection_intrinsic * D_intrinsic_point;
return projection;
}
/* ************************************************************************* */
}

48
geometry/SimpleCamera.h
View File

@ -37,17 +37,11 @@ namespace gtsam {
return K_;
}
/**
* project a 3d point to the camera and also check the depth
*/
std::pair<Point2,bool> projectSafe(const Point3& P) const;
/**
* project a 3d point to the camera
*/
Point2 project(const Point3& P) const;
/**
* backproject a 2d point from the camera up to a given scale
*/
@ -60,44 +54,16 @@ namespace gtsam {
*/
static SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
// Friends
friend Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
friend Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point);
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
*/
Point2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
};
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
* as well as the sign of the depth.
*/
std::pair<Point2, bool> projectSafe(const SimpleCamera& camera, const Point3& point);
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
*/
Point2 project(const SimpleCamera& camera, const Point3& point);
/**
* Derivatives of project.
*/
Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
/**
* super-duper combined evaluation + derivatives
* saves a lot of time because a lot of computation is shared
*/
Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point);
}
#endif /* SIMPLECAMERA_H_ */

163
geometry/StereoCamera.cpp
View File

@ -21,10 +21,69 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double ba
cy_ = calibration(4);
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point) const {
///* ************************************************************************* */
//StereoPoint2 StereoCamera::project(const Point3& point) const {
//
// Point3 cameraPoint = leftCamPose_.transform_to(point);
//
// double d = 1.0 / cameraPoint.z();
// double uL = cx_ + d * fx_ * cameraPoint.x();
// double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_);
// double v = cy_ + d * fy_ * cameraPoint.y();
// return StereoPoint2(uL,uR,v);
//}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> Dproject_stereo_pose,
boost::optional<Matrix&> Dproject_stereo_point) const {
Point3 cameraPoint = leftCamPose_.transform_to(point);
if (Dproject_stereo_pose) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused
//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
//**** above function call inlined
Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none);
//cout << "D_cameraPoint_pose" << endl;
//print(D_cameraPoint_pose);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
//cout << "myJacobian" << endl;
//print(D_intrinsic_cameraPoint);
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
}
if (Dproject_stereo_point) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused
//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
//**** above function call inlined
Matrix D_cameraPoint_point;
Point3 cameraPoint = pose().transform_to(point, boost::none, D_cameraPoint_point);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
}
Point3 cameraPoint = Pose3::transform_to(leftCamPose_, point);
double d = 1.0 / cameraPoint.z();
double uL = cx_ + d * fx_ * cameraPoint.x();
@ -34,14 +93,14 @@ StereoPoint2 StereoCamera::project(const Point3& point) const {
}
/* ************************************************************************* */
Matrix Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P) {
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d * d;
//return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x()-camera.baseline()) * d2, 0.0, d, -P.y() * d2);
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x()-baseline()) * d2, 0.0, d, -P.y() * d2);
}
/* ************************************************************************* */
Matrix Duncalibrate2(const Cal3_S2& K) {
Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
Vector calibration = K.vector(); // I want fx, fx, fy
Vector calibration2(3);
calibration2(0) = calibration(0);
@ -51,53 +110,53 @@ Matrix Duncalibrate2(const Cal3_S2& K) {
//return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
}
/* ************************************************************************* */
Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused
///* ************************************************************************* */
//Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
// //Point2 intrinsic = project(camera.calibrated_, point); // unused
//
// //Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
// //**** above function call inlined
// Matrix D_cameraPoint_pose;
// Point3 cameraPoint = camera.pose().transform_to(point, D_cameraPoint_pose, boost::none);
// //cout << "D_cameraPoint_pose" << endl;
// //print(D_cameraPoint_pose);
//
// //Point2 intrinsic = project_to_camera(cameraPoint); // unused
// Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
// //cout << "myJacobian" << endl;
// //print(D_intrinsic_cameraPoint);
//
// Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//
// //****
// //Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
// Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
//
// Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
// return D_projection_pose;
//}
//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
//**** above function call inlined
Matrix D_cameraPoint_pose;
Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, D_cameraPoint_pose, boost::none);
//cout << "D_cameraPoint_pose" << endl;
//print(D_cameraPoint_pose);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
//cout << "myJacobian" << endl;
//print(D_intrinsic_cameraPoint);
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
return D_projection_pose;
}
/* ************************************************************************* */
Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused
//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
//**** above function call inlined
Matrix D_cameraPoint_point;
Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, boost::none, D_cameraPoint_point);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
return D_projection_point;
}
///* ************************************************************************* */
//Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
// //Point2 intrinsic = project(camera.calibrated_, point); // unused
//
// //Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
// //**** above function call inlined
// Matrix D_cameraPoint_point;
// Point3 cameraPoint = camera.pose().transform_to(point, boost::none, D_cameraPoint_point);
//
// //Point2 intrinsic = project_to_camera(cameraPoint); // unused
// Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
//
// Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
//
// //****
// //Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
// Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
//
// Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
// return D_projection_point;
//}
// calibrated cameras

22
geometry/StereoCamera.h
View File

@ -43,7 +43,17 @@ namespace gtsam {
return baseline_;
}
StereoPoint2 project(const Point3& point) const;
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> Dproject_stereo_pose = boost::none,
boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
// Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
// Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
static Matrix Duncalibrate2(const Cal3_S2& K);
};
@ -57,15 +67,5 @@ namespace gtsam {
return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline());
}
inline StereoPoint2 project(const StereoCamera& camera, const Point3& point) {
return camera.project(point);
}
Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
Matrix
Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P);
Matrix Duncalibrate2(const Cal3_S2& K);
}

9
geometry/tests/testCal3_S2.cpp
View File

@ -36,18 +36,19 @@ TEST( Cal3_S2, calibrate)
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix computed = Duncalibrate1(K, p);
Matrix numerical = numericalDerivative21(uncalibrate, K, p);
Matrix computed; K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, Duncalibrate2)
{
Matrix computed = Duncalibrate2(K, p);
Matrix numerical = numericalDerivative22(uncalibrate, K, p);
Matrix computed; K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-9));
}

25
geometry/tests/testCalibratedCamera.cpp
View File

@ -71,37 +71,24 @@ TEST( CalibratedCamera, project)
}
/* ************************************************************************* */
TEST( CalibratedCamera, Dproject_to_camera1) {
Point3 pp(155,233,131);
Matrix test1 = Dproject_to_camera1(pp);
Matrix test2 = numericalDerivative11(project_to_camera, pp);
Matrix test1;
CalibratedCamera::project_to_camera(pp, test1);
Matrix test2 = numericalDerivative11<Point2,Point3>(
boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
CHECK(assert_equal(test1, test2));
}
/* ************************************************************************* */
Point2 project2(const Pose3& pose, const Point3& point) {
return project(CalibratedCamera(pose), point);
}
TEST( CalibratedCamera, Dproject_pose)
{
Matrix computed = Dproject_pose(camera, point1);
Matrix numerical = numericalDerivative21(project2, pose1, point1);
CHECK(assert_equal(computed, numerical,1e-7));
}
TEST( CalibratedCamera, Dproject_point)
{
Matrix computed = Dproject_point(camera, point1);
Matrix numerical = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(computed, numerical,1e-7));
return CalibratedCamera(pose).project(point);
}
TEST( CalibratedCamera, Dproject_point_pose)
{
Matrix Dpose, Dpoint;
Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-.16, .16) ));

5
geometry/tests/testPoint3.cpp
View File

@ -33,8 +33,9 @@ TEST( Point3, equals)
/* ************************************************************************* */
TEST( Point3, dot)
{
CHECK(Point3::dot(Point3(0,0,0),Point3(1,1,0)) == 0);
CHECK(Point3::dot(Point3(1,1,1),Point3(1,1,0)) == 2);
Point3 origin, ones(1,1,1);
CHECK(origin.dot(Point3(1,1,0)) == 0);
CHECK(ones.dot(Point3(1,1,0)) == 2);
}
/* ************************************************************************* */

65
geometry/tests/testPose2.cpp
View File

@ -117,6 +117,10 @@ TEST(Pose2, logmap) {
}
/* ************************************************************************* */
Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
return pose.transform_to(point);
}
TEST( Pose2, transform_to )
{
Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
@ -129,25 +133,29 @@ TEST( Pose2, transform_to )
// actual
Matrix actualH1, actualH2;
Point2 actual = Pose2::transform_to(pose,point, actualH1, actualH2);
Point2 actual = pose.transform_to(point, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1));
Matrix numericalH1 = numericalDerivative21(Pose2::transform_to, pose, point, 1e-5);
Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH2 = numericalDerivative22(Pose2::transform_to, pose, point, 1e-5);
Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
return pose.transform_from(point);
}
TEST (Pose2, transform_from)
{
Pose2 pose(1., 0., M_PI_2);
Point2 pt(2., 1.);
Matrix H1, H2;
Point2 actual = Pose2::transform_from(pose, pt, H1, H2);
Point2 actual = pose.transform_from(pt, H1, H2);
Point2 expected(0., 2.);
CHECK(assert_equal(expected, actual));
@ -155,11 +163,11 @@ TEST (Pose2, transform_from)
Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(Pose2::transform_from, pose, pt, 1e-5);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H1_expected, H1));
CHECK(assert_equal(H1_expected, numericalH1));
Matrix numericalH2 = numericalDerivative22(Pose2::transform_from, pose, pt, 1e-5);
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H2_expected, H2));
CHECK(assert_equal(H2_expected, numericalH2));
}
@ -193,8 +201,8 @@ TEST(Pose2, compose_a)
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = Pose2::transform_to(pose1 * pose2, point);
Point2 actual_point2 = Pose2::transform_to(pose2, Pose2::transform_to(pose1, point));
Point2 actual_point1 = (pose1 * pose2).transform_to(point);
Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point));
CHECK(assert_equal(expected_point, actual_point1));
CHECK(assert_equal(expected_point, actual_point2));
}
@ -345,7 +353,7 @@ TEST( Pose2, between )
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
CHECK(assert_equal(-AdjointMap(between(gT2,gT1)),actualH1));
CHECK(assert_equal(-between(gT2,gT1).AdjointMap(),actualH1));
Matrix expectedH2 = Matrix_(3,3,
1.0, 0.0, 0.0,
@ -395,65 +403,72 @@ Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
return pose.bearing(pt);
}
TEST( Pose2, bearing )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish bearing is indeed zero
CHECK(assert_equal(Rot2(),bearing(x1,l1)));
CHECK(assert_equal(Rot2(),x1.bearing(l1)));
// establish bearing is indeed 45 degrees
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),bearing(x1,l2)));
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2)));
// establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5);
expectedH2 = numericalDerivative22(bearing_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
// establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5);
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
}
/* ************************************************************************* */
double range_proxy(const Pose2& pose, const Point2& point) {
return pose.range(point);
}
TEST( Pose2, range )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish range is indeed zero
DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9);
DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed 45 degrees
DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9);
DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9);
// Another pair
double actual23 = gtsam::range(x2, l3, actualH1, actualH2);
double actual23 = x2.range(l3, actualH1, actualH2);
DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range, x2, l3, 1e-5);
expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(range, x2, l3, 1e-5);
expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
// Another test
double actual34 = gtsam::range(x3, l4, actualH1, actualH2);
double actual34 = x3.range(l4, actualH1, actualH2);
DOUBLES_EQUAL(2,actual34,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(range, x3, l4, 1e-5);
expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
}

52
geometry/tests/testPose3.cpp
View File

@ -216,11 +216,12 @@ TEST( Pose3, compose_inverse)
}
/* ************************************************************************* */
Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); }
TEST( Pose3, Dtransform_from1_a)
{
Matrix actualDtransform_from1;
Pose3::transform_from(T, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,T,P);
T.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -228,8 +229,8 @@ TEST( Pose3, Dtransform_from1_b)
{
Pose3 origin;
Matrix actualDtransform_from1;
Pose3::transform_from(origin, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,origin,P);
origin.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -238,8 +239,8 @@ TEST( Pose3, Dtransform_from1_c)
Point3 origin;
Pose3 T0(R,origin);
Matrix actualDtransform_from1;
Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
T0.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -249,9 +250,9 @@ TEST( Pose3, Dtransform_from1_d)
Point3 t0(100,0,0);
Pose3 T0(I,t0);
Matrix actualDtransform_from1;
Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
T0.transform_from(P, actualDtransform_from1, boost::none);
//print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
Matrix numerical = numericalDerivative21(transform_from_,T0,P);
//print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -260,17 +261,18 @@ TEST( Pose3, Dtransform_from1_d)
TEST( Pose3, Dtransform_from2)
{
Matrix actualDtransform_from2;
Pose3::transform_from(T,P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(Pose3::transform_from,T,P);
T.transform_from(P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from_,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
}
/* ************************************************************************* */
Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); }
TEST( Pose3, Dtransform_to1)
{
Matrix computed;
Pose3::transform_to(T, P, computed, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_to,T,P);
T.transform_to(P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to_,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
@ -278,8 +280,8 @@ TEST( Pose3, Dtransform_to1)
TEST( Pose3, Dtransform_to2)
{
Matrix computed;
Pose3::transform_to(T,P, boost::none, computed);
Matrix numerical = numericalDerivative22(Pose3::transform_to,T,P);
T.transform_to(P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to_,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
@ -287,9 +289,9 @@ TEST( Pose3, Dtransform_to2)
TEST( Pose3, transform_to_with_derivatives)
{
Matrix actH1, actH2;
Pose3::transform_to(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(Pose3::transform_to, T,P),
expH2 = numericalDerivative22(Pose3::transform_to, T,P);
T.transform_to(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to_, T,P),
expH2 = numericalDerivative22(transform_to_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
}
@ -298,9 +300,9 @@ TEST( Pose3, transform_to_with_derivatives)
TEST( Pose3, transform_from_with_derivatives)
{
Matrix actH1, actH2;
Pose3::transform_from(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(Pose3::transform_from, T,P),
expH2 = numericalDerivative22(Pose3::transform_from, T,P);
T.transform_from(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_from_, T,P),
expH2 = numericalDerivative22(transform_from_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
}
@ -308,7 +310,7 @@ TEST( Pose3, transform_from_with_derivatives)
/* ************************************************************************* */
TEST( Pose3, transform_to_translate)
{
Point3 actual = Pose3::transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.));
Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.));
Point3 expected(9.,18.,27.);
CHECK(assert_equal(expected, actual));
}
@ -317,7 +319,7 @@ TEST( Pose3, transform_to_translate)
TEST( Pose3, transform_to_rotate)
{
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3());
Point3 actual = Pose3::transform_to(transform, Point3(2,1,10));
Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10);
CHECK(assert_equal(expected, actual, 0.001));
}
@ -326,7 +328,7 @@ TEST( Pose3, transform_to_rotate)
TEST( Pose3, transform_to)
{
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0));
Point3 actual = Pose3::transform_to(transform, Point3(3,2,10));
Point3 actual = transform.transform_to(Point3(3,2,10));
Point3 expected(2,1,10);
CHECK(assert_equal(expected, actual, 0.001));
}
@ -334,7 +336,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */
TEST( Pose3, transform_from)
{
Point3 actual = Pose3::transform_from(T3, Point3());
Point3 actual = T3.transform_from(Point3());
Point3 expected = Point3(1.,2.,3.);
CHECK(assert_equal(expected, actual));
}
@ -342,7 +344,7 @@ TEST( Pose3, transform_from)
/* ************************************************************************* */
TEST( Pose3, transform_roundtrip)
{
Point3 actual = Pose3::transform_from(T3, Pose3::transform_to(T3, Point3(12., -0.11,7.0)));
Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0)));
Point3 expected(12., -0.11,7.0);
CHECK(assert_equal(expected, actual));
}

11
geometry/tests/testRot2.cpp
View File

@ -66,7 +66,7 @@ inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}
TEST( Rot2, rotate)
{
Matrix H1, H2;
Point2 actual = Rot2::rotate(R, P, H1, H2);
Point2 actual = R.rotate(P, H1, H2);
CHECK(assert_equal(actual,R*P));
Matrix numerical1 = numericalDerivative21(rotate_, R, P);
CHECK(assert_equal(numerical1,H1));
@ -76,11 +76,11 @@ TEST( Rot2, rotate)
/* ************************************************************************* */
// unrotate and derivatives
inline Point2 unrotate_(const Rot2 & R, const Point2& p) {return R.unrotate(p);}
inline Point2 unrotate_(const Rot2& R, const Point2& p) {return R.unrotate(p);}
TEST( Rot2, unrotate)
{
Matrix H1, H2;
Point2 w = R * P, actual = Rot2::unrotate(R, w, H1, H2);
Point2 w = R * P, actual = R.unrotate(w, H1, H2);
CHECK(assert_equal(actual,P));
Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
CHECK(assert_equal(numerical1,H1));
@ -89,6 +89,7 @@ TEST( Rot2, unrotate)
}
/* ************************************************************************* */
inline Rot2 relativeBearing_(const Point2& pt) {return Rot2::relativeBearing(pt); }
TEST( Rot2, relativeBearing )
{
Point2 l1(1, 0), l2(1, 1);
@ -99,7 +100,7 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(Rot2(),actual1));
// Check numerical derivative
expectedH = numericalDerivative11(Rot2::relativeBearing, l1, 1e-5);
expectedH = numericalDerivative11(relativeBearing_, l1, 1e-5);
CHECK(assert_equal(expectedH,actualH));
// establish relativeBearing is indeed 45 degrees
@ -107,7 +108,7 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2));
// Check numerical derivative
expectedH = numericalDerivative11(Rot2::relativeBearing, l2, 1e-5);
expectedH = numericalDerivative11(relativeBearing_, l2, 1e-5);
CHECK(assert_equal(expectedH,actualH));
}

20
geometry/tests/testRot3.cpp
View File

@ -207,7 +207,7 @@ public:
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return AngularVelocity::cross(X, Y);
return X.cross(Y);
}
/* ************************************************************************* */
@ -224,31 +224,33 @@ TEST(Rot3, BCH)
}
/* ************************************************************************* */
inline Point3 rotate_(const Rot3& r, const Point3& pt) { return r.rotate(pt); }
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
Rot3::rotate(R, P, actualDrotate1a, actualDrotate2);
Rot3::rotate(R.inverse(), P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(Rot3::rotate, R, P);
Matrix numerical2 = numericalDerivative21(Rot3::rotate, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(Rot3::rotate, R, P);
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(rotate_, R, P);
Matrix numerical2 = numericalDerivative21(rotate_, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(rotate_, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
inline Point3 unrotate_(const Rot3& r, const Point3& pt) { return r.unrotate(pt); }
TEST( Rot3, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = Rot3::unrotate(R,w,H1,H2);
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(Rot3::unrotate, R, w);
Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(Rot3::unrotate, R, w);
Matrix numerical2 = numericalDerivative22(unrotate_, R, w);
CHECK(assert_equal(numerical2,H2,error));
}

19
geometry/tests/testSimpleCamera.cpp
View File

@ -76,7 +76,6 @@ TEST( SimpleCamera, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(K, Pose3(rot, origin));
Point3 actual = camera.backproject(Point2(), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
@ -88,27 +87,13 @@ TEST( SimpleCamera, backproject2)
/* ************************************************************************* */
Point2 project2(const Pose3& pose, const Point3& point) {
return project(SimpleCamera(K,pose), point);
}
TEST( SimpleCamera, Dproject_pose)
{
Matrix computed = Dproject_pose(camera, point1);
Matrix numerical = numericalDerivative21(project2, pose1, point1);
CHECK(assert_equal(computed, numerical,1e-7));
}
TEST( SimpleCamera, Dproject_point)
{
Matrix computed = Dproject_point(camera, point1);
Matrix numerical = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(computed, numerical,1e-7));
return SimpleCamera(K,pose).project(point);
}
TEST( SimpleCamera, Dproject_point_pose)
{
Matrix Dpose, Dpoint;
Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));

9
geometry/tests/testStereoCamera.cpp
View File

@ -76,18 +76,19 @@ StereoCamera stereoCam(Pose3(), K, 0.5);
Point3 p(0, 0, 5);
/* ************************************************************************* */
StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
TEST( StereoCamera, Dproject_stereo_pose)
{
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project,stereoCam, p);
Matrix actual = Dproject_stereo_pose(stereoCam, p);
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
Matrix actual; stereoCam.project(p, actual, boost::none);
CHECK(assert_equal(expected,actual,1e-7));
}
/* ************************************************************************* */
TEST( StereoCamera, Dproject_stereo_point)
{
Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project,stereoCam, p);
Matrix actual = Dproject_stereo_point(stereoCam, p);
Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
Matrix actual; stereoCam.project(p, boost::none, actual);
CHECK(assert_equal(expected,actual,1e-8));
}

47
geometry/timeCalibratedCamera.cpp
View File

@ -26,37 +26,42 @@ int main()
const CalibratedCamera camera(pose1);
const Point3 point1(-0.08,-0.08, 0.0);
/**
* NOTE: because we only have combined derivative functions now,
* parts of this test are no longer useful.
*/
// Aug 8, iMac 3.06GHz Core i3
// 378870 calls/second
// 2.63943 musecs/call
// AFTER collapse:
// 1.57399e+06 calls/second
// 0.63533 musecs/call
{
Matrix computed;
long timeLog = clock();
for(int i = 0; i < n; i++)
computed = Dproject_pose(camera, point1);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
}
// {
// Matrix computed;
// long timeLog = clock();
// for(int i = 0; i < n; i++)
// computed = Dproject_pose(camera, point1);
// long timeLog2 = clock();
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
// cout << ((double)n/seconds) << " calls/second" << endl;
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
// }
// Aug 8, iMac 3.06GHz Core i3
// AFTER collapse:
// 1.55383e+06 calls/second
// 0.64357 musecs/call
{
Matrix computed;
long timeLog = clock();
for(int i = 0; i < n; i++)
computed = Dproject_point(camera, point1);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
}
// {
// Matrix computed;
// long timeLog = clock();
// for(int i = 0; i < n; i++)
// computed = Dproject_point(camera, point1);
// long timeLog2 = clock();
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
// cout << ((double)n/seconds) << " calls/second" << endl;
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
// }
// Aug 8, iMac 3.06GHz Core i3
// 371153 calls/second
@ -68,7 +73,7 @@ int main()
Matrix computed1, computed2;
long timeLog = clock();
for(int i = 0; i < n; i++)
Dproject_pose_point(camera, point1, computed1, computed2);
camera.project(point1, computed1, computed2);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;

1
nonlinear/LieConfig-inl.h
View File

@ -14,6 +14,7 @@
#include <stdexcept>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/LieConfig.h>

2
slam/BearingFactor.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 hx = bearing(pose, point, H1, H2);
Rot2 hx = pose.bearing(point, H1, H2);
return logmap(between(z_, hx));
}

4
slam/BearingRangeFactor.h
View File

@ -44,10 +44,10 @@ namespace gtsam {
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot2 y1 = gtsam::bearing(pose, point, H11_, H12_);
Rot2 y1 = pose.bearing(point, H11_, H12_);
Vector e1 = logmap(between(bearing_, y1));
double y2 = gtsam::range(pose, point, H21_, H22_);
double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21);

2
slam/RangeFactor.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
/** h(x)-z */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = gtsam::range(pose, point, H1, H2);
double hx = pose.range(point, H1, H2);
return Vector_(1, hx - z_);
}

1
slam/Simulated3D.h
View File

@ -9,6 +9,7 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h>

6
slam/TransformConstraint.h
View File

@ -48,19 +48,19 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
if (Dforeign) {
Matrix Af;
Transform::transform_from(trans, foreign, boost::none, Af);
trans.transform_from(foreign, boost::none, Af);
*Dforeign = Af;
}
if (Dtrans) {
Matrix At;
Transform::transform_from(trans, foreign, At, boost::none);
trans.transform_from(foreign, At, boost::none);
*Dtrans = At;
}
if (Dlocal) {
Point dummy;
*Dlocal = -1* eye(dummy.dim());
}
return gtsam::logmap(gtsam::between<Point>(local, Transform::transform_from(trans, foreign)));
return gtsam::logmap(gtsam::between<Point>(local, trans.transform_from(foreign)));
}
};
} // \namespace gtsam

1
slam/pose2SLAM.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/slam/PriorFactor.h>

4
slam/visualSLAM.h
View File

@ -98,9 +98,7 @@ namespace gtsam { namespace visualSLAM {
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
SimpleCamera camera(*K_, pose);
if (H1) *H1=Dproject_pose(camera,point);
if (H2) *H2=Dproject_point(camera,point);
Point2 reprojectionError(project(camera, point) - z_);
Point2 reprojectionError(camera.project(point, H1, H2) - z_);
return reprojectionError.vector();
}

10
tests/testTransformConstraint.cpp
View File

@ -94,7 +94,7 @@ TEST( TransformConstraint, jacobians_zero ) {
// get values that are ideal
Pose2 trans(2.0, 3.0, 0.0);
Point2 global(5.0, 6.0);
Point2 local = Pose2::transform_from(trans, global);
Point2 local = trans.transform_from(global);
PointTransformConstraint tc(lA1, t1, lB1);
Vector actCost = tc.evaluateError(global, trans, local),
@ -129,8 +129,8 @@ TEST( TransformConstraint, converge_trans ) {
Pose2 transIdeal(7.0, 3.0, M_PI/2);
// verify direction
CHECK(assert_equal(local1, Pose2::transform_from(transIdeal, global1)));
CHECK(assert_equal(local2, Pose2::transform_from(transIdeal, global2)));
CHECK(assert_equal(local1, transIdeal.transform_from(global1)));
CHECK(assert_equal(local2, transIdeal.transform_from(global2)));
// choose transform
// Pose2 trans = transIdeal; // ideal - works
@ -187,7 +187,7 @@ TEST( TransformConstraint, converge_local ) {
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealLocal = Pose2::transform_from(trans, global);
Point2 idealLocal = trans.transform_from(global);
// perturb the initial estimate
// Point2 local = idealLocal; // Ideal case - works
@ -226,7 +226,7 @@ TEST( TransformConstraint, converge_global ) {
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealForeign = Pose2::transform_from(inverse(trans), local);
Point2 idealForeign = inverse(trans).transform_from(local);
// perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works