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

Merged in feature/PinholePose (pull request #108) 

Revamped Pinhole Cameras
release/4.3a0
Frank Dellaert 2015-02-21 18:15:18 +01:00
parent 654d563d63 61ae24508d
commit a9d8a915ac
18 changed files with 1109 additions and 410 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
.cproject
View File

@ -1019,6 +1019,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholePose.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testPinholePose.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1285,7 +1293,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1325,7 +1332,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1333,7 +1339,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1437,6 +1442,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1739,6 +1745,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1746,6 +1753,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1753,6 +1761,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1760,6 +1769,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1951,7 +1961,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2103,6 +2112,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2110,6 +2120,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2157,6 +2168,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2164,6 +2176,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2171,6 +2184,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2186,6 +2200,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3305,6 +3320,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3312,6 +3328,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3319,6 +3336,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

1
examples/Pose2SLAMExample_g2o.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>

1
examples/Pose2SLAMExample_lago.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
using namespace std;

24
gtsam/discrete/DecisionTree-inl.h
View File

@ -462,15 +462,17 @@ namespace gtsam {
// cannot just create a root Choice node on the label: if the label is not the
// highest label, we need to do a complicated and expensive recursive call.
template<typename L, typename Y> template<typename Iterator>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(
Iterator begin, Iterator end, const L& label) const {
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(Iterator begin,
Iterator end, const L& label) const {
// find highest label among branches
boost::optional<L> highestLabel;
boost::optional<size_t> nrChoices;
for (Iterator it = begin; it != end; it++) {
if (it->root_->isLeaf()) continue;
boost::shared_ptr<const Choice> c = boost::dynamic_pointer_cast<const Choice> (it->root_);
if (it->root_->isLeaf())
continue;
boost::shared_ptr<const Choice> c =
boost::dynamic_pointer_cast<const Choice>(it->root_);
if (!highestLabel || c->label() > *highestLabel) {
highestLabel.reset(c->label());
nrChoices.reset(c->nrChoices());
@ -478,15 +480,15 @@ namespace gtsam {
}
// if label is already in correct order, just put together a choice on label
if (!highestLabel || label > *highestLabel) {
if (!highestLabel || !nrChoices || label > *highestLabel) {
boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
for (Iterator it = begin; it != end; it++)
choiceOnLabel->push_back(it->root_);
return Choice::Unique(choiceOnLabel);
}
} else {
// Set up a new choice on the highest label
boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices));
boost::shared_ptr<Choice> choiceOnHighestLabel(
new Choice(*highestLabel, *nrChoices));
// now, for all possible values of highestLabel
for (size_t index = 0; index < *nrChoices; index++) {
// make a new set of functions for composing by iterating over the given
@ -503,6 +505,7 @@ namespace gtsam {
}
return Choice::Unique(choiceOnHighestLabel);
}
}
/*********************************************************************************/
// "create" is a bit of a complicated thing, but very useful.
@ -667,9 +670,10 @@ namespace gtsam {
void DecisionTree<L, Y>::dot(const std::string& name, bool showZero) const {
std::ofstream os((name + ".dot").c_str());
dot(os, showZero);
system(
int result = system(
("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str());
}
if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed");
}
/*********************************************************************************/

165
gtsam/geometry/CalibratedCamera.cpp
View File

@ -19,88 +19,131 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/CalibratedCamera.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) {
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
return Dpn_pose;
}
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) :
pose_(Pose3::Expmap(v)) {
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
/**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
Dpn_point *= d;
return Dpn_point;
}
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
OptionalJacobian<2,3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
return Pose3(wRc, t);
}
/* ************************************************************************* */
Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
return Pose3(Rot3(xc, yc, zc), eye);
}
/* ************************************************************************* */
bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void PinholeBase::print(const string& s) const {
pose_.print(s + ".pose");
}
/* ************************************************************************* */
const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return Point2(P.x() / P.z(), P.y() / P.z());
return pose_;
}
/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
Point2 PinholeBase::project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
return make_pair(pn, pc.z() > 0);
}
/* ************************************************************************* */
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt);
}
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
/* ************************************************************************* */
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);
Rot3 wRc(x, y, z);
Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t);
return CalibratedCamera(pose3);
return CalibratedCamera(LevelPose(pose2, height));
}
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
const Point3& target, const Point3& upVector) {
return CalibratedCamera(LookatPose(eye, target, upVector));
}
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Matrix36 Dpose_;
Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else
Point3 q = pose_.transform_to(point);
#endif
Point2 intrinsic = project_to_camera(q);
// Check if point is in front of camera
if (q.z() <= 0)
throw CheiralityException();
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
if(Dpose && Dpoint) {
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
}
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v;
if (Dpoint) {
const Matrix3 R(pose_.rotation().matrix());
Matrix23 Dpoint_;
Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
*Dpoint = d * Dpoint_;
}
#endif
}
return intrinsic;
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint);
}
/* ************************************************************************* */

282
gtsam/geometry/CalibratedCamera.h
View File

@ -19,10 +19,11 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
class Point2;
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> {
public:
@ -31,6 +32,146 @@ public:
}
};
/**
* A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT PinholeBase {
private:
Pose3 pose_; ///< 3D pose of camera
protected:
/// @name Derivatives
/// @{
/**
* Calculate Jacobian with respect to pose
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
*/
static Matrix26 Dpose(const Point2& pn, double d);
/**
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Rt transposed rotation matrix
*/
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
/// @}
public:
/// @name Static functions
/// @{
/**
* Create a level pose at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static Pose3 LevelPose(const Pose2& pose2, double height);
/**
* Create a camera pose at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector);
/// @}
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBase() {
}
/** constructor with pose */
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const PinholeBase &camera, double tol = 1e-9) const;
/// print
void print(const std::string& s = "PinholeBase") const;
/// @}
/// @name Standard Interface
/// @{
/// return pose, constant version
const Pose3& pose() const {
return pose_;
}
/// return pose, with derivative
const Pose3& getPose(OptionalJacobian<6, 6> H) const;
/// @}
/// @name Transformations and measurement functions
/// @{
/**
* Project from 3D point in camera coordinates into image
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 project_to_camera(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
/**
* Project point into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Point2& p, const double depth);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
}
};
// end of class PinholeBase
/**
* A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient
@ -38,13 +179,13 @@ public:
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT CalibratedCamera {
private:
Pose3 pose_; // 6DOF pose
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
public:
enum { dimension = 6 };
enum {
dimension = 6
};
/// @name Standard Constructors
/// @{
@ -54,26 +195,40 @@ public:
}
/// construct with pose
explicit CalibratedCamera(const Pose3& pose);
explicit CalibratedCamera(const Pose3& pose) :
PinholeBase(pose) {
}
/// @}
/// @name Named Constructors
/// @{
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
* @param height specifies the height of the camera (along the positive Z-axis)
* (theta 0 = looking in direction of positive X axis)
*/
static CalibratedCamera Level(const Pose2& pose2, double height);
/**
* Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector);
/// @}
/// @name Advanced Constructors
/// @{
/// construct from vector
explicit CalibratedCamera(const Vector &v);
/// @}
/// @name Testable
/// @{
virtual void print(const std::string& s = "") const {
pose_.print(s);
}
/// check equality to another camera
bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol);
explicit CalibratedCamera(const Vector &v) :
PinholeBase(v) {
}
/// @}
@ -84,19 +239,6 @@ public:
virtual ~CalibratedCamera() {
}
/// return pose
inline const Pose3& pose() const {
return pose_;
}
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
* @param height specifies the height of the camera (along the positive Z-axis)
* (theta 0 = looking in direction of positive X axis)
*/
static CalibratedCamera Level(const Pose2& pose2, double height);
/// @}
/// @name Manifold
/// @{
@ -107,87 +249,68 @@ public:
/// Return canonical coordinate
Vector localCoordinates(const CalibratedCamera& T2) const;
/// Lie group dimensionality
/// @deprecated
inline size_t dim() const {
return 6;
}
/// Lie group dimensionality
/// @deprecated
inline static size_t Dim() {
return 6;
}
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
/// @}
/// @name Transformations and mesaurement functions
/// @{
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
* @param point a 3D point to be projected
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the 3D point
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const Point3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative
* @deprecated
* Use project2, which is more consistently named across Pinhole cameras
*/
static Point2 project_to_camera(const Point3& cameraPoint,
OptionalJacobian<2, 3> H1 = boost::none);
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
static Point3 backproject_from_camera(const Point2& p, const double scale);
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& pn, double depth) const {
return pose().transform_from(backproject_from_camera(pn, depth));
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
return pose().range(point, Dcamera, Dpoint);
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2);
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
return this->pose().range(pose, Dcamera, Dpose);
}
/**
* Calculate range to another camera
* @param camera Other camera
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2);
double range(const CalibratedCamera& camera, //
OptionalJacobian<1, 6> H1 = boost::none, //
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose().range(camera.pose(), H1, H2);
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
@ -195,17 +318,22 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
}
/// @}
};
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
}

299
gtsam/geometry/PinholeCamera.h
View File

@ -18,32 +18,32 @@
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
#include <gtsam/geometry/PinholePose.h>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* Use PinholePose if you will not be optimizing for Calibration
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera {
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
private:
Pose3 pose_;
Calibration K_;
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
Calibration K_; ///< Calibration, part of class now
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value;
public:
enum { dimension = 6 + DimK };
enum {
dimension = 6 + DimK
}; ///< Dimension depends on calibration
/// @name Standard Constructors
/// @{
@ -54,12 +54,12 @@ public:
/** constructor with pose */
explicit PinholeCamera(const Pose3& pose) :
pose_(pose) {
Base(pose) {
}
/** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& K) :
pose_(pose), K_(K) {
Base(pose), K_(K) {
}
/// @}
@ -75,12 +75,7 @@ public:
*/
static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
const Pose3 pose3(wRc, t);
return PinholeCamera(pose3, K);
return PinholeCamera(Base::LevelPose(pose2, height), K);
}
/// PinholeCamera::level with default calibration
@ -99,28 +94,23 @@ public:
*/
static PinholeCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const Calibration& K = Calibration()) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
Pose3 pose3(Rot3(xc, yc, zc), eye);
return PinholeCamera(pose3, K);
return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeCamera(const Vector &v) {
pose_ = Pose3::Expmap(v.head(6));
if (v.size() > 6) {
K_ = Calibration(v.tail(DimK));
}
/// Init from vector, can be 6D (default calibration) or dim
explicit PinholeCamera(const Vector &v) :
Base(v.head<6>()) {
if (v.size() > 6)
K_ = Calibration(v.tail<DimK>());
}
/// Init from Vector and calibration
PinholeCamera(const Vector &v, const Vector &K) :
pose_(Pose3::Expmap(v)), K_(K) {
Base(v), K_(K) {
}
/// @}
@ -128,14 +118,14 @@ public:
/// @{
/// assert equality up to a tolerance
bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol)
&& K_.equals(camera.calibration(), tol);
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
}
/// print
void print(const std::string& s = "PinholeCamera") const {
pose_.print(s + ".pose");
Base::print(s);
K_.print(s + ".calibration");
}
@ -147,31 +137,21 @@ public:
}
/// return pose
inline Pose3& pose() {
return pose_;
}
/// return pose, constant version
inline const Pose3& pose() const {
return pose_;
const Pose3& pose() const {
return Base::pose();
}
/// return pose, with derivative
inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return pose_;
return Base::pose();
}
/// return calibration
inline Calibration& calibration() {
return K_;
}
/// return calibration
inline const Calibration& calibration() const {
const Calibration& calibration() const {
return K_;
}
@ -179,13 +159,13 @@ public:
/// @name Manifold
/// @{
/// Manifold dimension
inline size_t dim() const {
/// @deprecated
size_t dim() const {
return dimension;
}
/// Manifold dimension
inline static size_t Dim() {
/// @deprecated
static size_t Dim() {
return dimension;
}
@ -194,9 +174,9 @@ public:
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == 6)
return PinholeCamera(pose().retract(d), calibration());
return PinholeCamera(this->pose().retract(d), calibration());
else
return PinholeCamera(pose().retract(d.head(6)),
return PinholeCamera(this->pose().retract(d.head(6)),
calibration().retract(d.tail(calibration().dim())));
}
@ -204,7 +184,7 @@ public:
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = pose().localCoordinates(T2.pose());
d.head(6) = this->pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
return d;
}
@ -218,32 +198,6 @@ public:
/// @name Transformations and measurement functions
/// @{
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/
static Point2 project_to_camera(const Point3& P, //
OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
#endif
double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/// Project a point into the image and check depth
inline std::pair<Point2, bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
}
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image
@ -252,31 +206,25 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
if (Dpose || Dpoint) {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// chain the Jacobian matrices
// If needed, apply chain rule
if (Dpose)
calculateDpose(pn, d, Dpi_pn, *Dpose);
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
*Dpoint = Dpi_pn * *Dpoint;
return pi;
} else
return K_.uncalibrate(pn, Dcal);
}
/** project a point at infinity from world coordinate to the image
@ -285,20 +233,19 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 projectPointAtInfinity(const Point3& pw,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = project_to_camera(pc); // project the point to the camera
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
@ -306,7 +253,7 @@ public:
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn; // 2*2
@ -323,65 +270,40 @@ public:
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
// project to normalized coordinates
Matrix26 Dpose;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
if (!Dcamera && !Dpoint) {
return K_.uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
// uncalibrate to pixel coordinates
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
Dcamera || Dpoint ? &Dpi_pn : 0);
// If needed, calculate derivatives
if (Dcamera)
*Dcamera << Dpi_pn * Dpose, Dcal;
if (Dpoint)
*Dpoint = Dpi_pn * (*Dpoint);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
(*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
inline Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
return pose_.transform_from(pc);
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
inline Point3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose_.rotation().rotate(pc);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double)
*/
double range(
const Point3& point, //
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
@ -390,16 +312,12 @@ public:
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double)
*/
double range(
const Pose3& pose, //
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
@ -408,26 +326,21 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
template<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const {
boost::optional<Matrix&> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6+CalibrationB::dimension);
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
}
@ -437,12 +350,9 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
double range(
const CalibratedCamera& camera, //
double range(const CalibratedCamera& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
@ -450,65 +360,26 @@ public:
private:
/**
* Calculate Jacobian with respect to pose
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpose Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
Eigen::MatrixBase<Derived> const & Dpose) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
Dpi_pn * Dpn_pose;
}
/**
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpoint Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn * Dpn_point;
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
template<typename Calibration>
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
template<typename Calibration>
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
template<typename Calibration>
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
} // \ gtsam

344
gtsam/geometry/PinholePose.h Normal file
View File

@ -0,0 +1,344 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PinholePose.h
* @brief Pinhole camera with known calibration
* @author Yong-Dian Jian
* @author Frank Dellaert
* @date Feb 20, 2015
*/
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
public :
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBaseK() {
}
/** constructor with pose */
explicit PinholeBaseK(const Pose3& pose) :
PinholeBase(pose) {
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBaseK(const Vector &v) :
PinholeBase(v) {
}
/// @}
/// @name Standard Interface
/// @{
virtual ~PinholeBaseK() {
}
/// return calibration
virtual const Calibration& calibration() const = 0;
/// @}
/// @name Transformations and measurement functions
/// @{
/// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
pn.first = calibration().uncalibrate(pn.first);
return pn;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinates
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, boost::none,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose) *Dpose = Dpi_pn * (*Dpose);
if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
return pi;
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);
return pose().transform_from(backproject_from_camera(pn, depth));
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Point3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose().rotation().rotate(pc);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
return pose().range(point, Dcamera, Dpoint);
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
return this->pose().range(pose, Dcamera, Dpose);
}
/**
* Calculate range to a CalibratedCamera
* @param camera Other camera
* @return range (double)
*/
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
/**
* Calculate range to a PinholePoseK derived class
* @param camera Other camera
* @return range (double)
*/
template<class CalibrationB>
double range(const PinholeBaseK<CalibrationB>& camera,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
}
};
// end of class PinholeBaseK
/**
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* Instead of using this class, one might consider calibrating the measurements
* and using CalibratedCamera, which would then be faster.
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
public:
enum {
dimension = 6
}; ///< There are 6 DOF to optimize for
/// @name Standard Constructors
/// @{
/** default constructor */
PinholePose() {
}
/** constructor with pose, uses default calibration */
explicit PinholePose(const Pose3& pose) :
Base(pose), K_(new Calibration()) {
}
/** constructor with pose and calibration */
PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
Base(pose), K_(K) {
}
/// @}
/// @name Named Constructors
/// @{
/**
* Create a level camera at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static PinholePose Level(const boost::shared_ptr<Calibration>& K,
const Pose2& pose2, double height) {
return PinholePose(Base::LevelPose(pose2, height), K);
}
/// PinholePose::level with default calibration
static PinholePose Level(const Pose2& pose2, double height) {
return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
}
/**
* Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
* @param K optional calibration parameter
*/
static PinholePose Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const boost::shared_ptr<Calibration>& K =
boost::make_shared<Calibration>()) {
return PinholePose(Base::LookatPose(eye, target, upVector), K);
}
/// @}
/// @name Advanced Constructors
/// @{
/// Init from 6D vector
explicit PinholePose(const Vector &v) :
Base(v), K_(new Calibration()) {
}
/// Init from Vector and calibration
PinholePose(const Vector &v, const Vector &K) :
Base(v), K_(new Calibration(K)) {
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
}
/// print
void print(const std::string& s = "PinholePose") const {
Base::print(s);
K_->print(s + ".calibration");
}
/// @}
/// @name Standard Interface
/// @{
virtual ~PinholePose() {
}
/// return calibration
virtual const Calibration& calibration() const {
return *K_;
}
/// @}
/// @name Manifold
/// @{
/// @deprecated
size_t dim() const {
return 6;
}
/// @deprecated
static size_t Dim() {
return 6;
}
/// move a cameras according to d
PinholePose retract(const Vector6& d) const {
return PinholePose(Base::pose().retract(d), K_);
}
/// return canonical coordinate
Vector6 localCoordinates(const PinholePose& p) const {
return Base::pose().localCoordinates(p.Base::pose());
}
/// for Canonical
static PinholePose identity() {
return PinholePose(); // assumes that the default constructor is valid
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
// end of class PinholePose
template<typename Calibration>
struct traits<PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
};
template<typename Calibration>
struct traits<const PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
};
} // \ gtsam

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

@ -29,6 +29,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) {
}
/* ************************************************************************* */
static Point2 project2(const Pose3& pose, const Point3& point) {
return CalibratedCamera(pose).project(point);
static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
return camera.project(point);
}
TEST( CalibratedCamera, Dproject_point_pose)
{
Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
CHECK(assert_equal(Point2(-.16, .16), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint;
camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -15,12 +15,14 @@
* @brief test PinholeCamera class
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2)
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( PinholeCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);

259
gtsam/geometry/tests/testPinholePose.cpp Normal file
View File

@ -0,0 +1,259 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPinholePose.cpp
* @author Frank Dellaert
* @brief test PinholePose class
* @date Feb 20, 2015
*/
#include <gtsam/geometry/PinholePose.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
typedef PinholePose<Cal3_S2> Camera;
static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1, K);
static const Point3 point1(-0.08,-0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
static const Point3 point3( 0.08, 0.08, 0.0);
static const Point3 point4( 0.08,-0.08, 0.0);
static const Point3 point1_inf(-0.16,-0.16, -1.0);
static const Point3 point2_inf(-0.16, 0.16, -1.0);
static const Point3 point3_inf( 0.16, 0.16, -1.0);
static const Point3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholePose, constructor)
{
EXPECT(assert_equal( pose, camera.pose()));
}
//******************************************************************************
TEST(PinholeCamera, Pose) {
Matrix actualH;
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
/* ************************************************************************* */
TEST( PinholePose, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
}
/* ************************************************************************* */
TEST( PinholePose, project)
{
EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) ));
EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) ));
EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) ));
EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) ));
}
/* ************************************************************************* */
TEST( PinholePose, backproject)
{
EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
}
/* ************************************************************************* */
TEST( PinholePose, backprojectInfinity)
{
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
}
/* ************************************************************************* */
TEST( PinholePose, backproject2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(x.second);
}
/* ************************************************************************* */
static Point2 project3(const Pose3& pose, const Point3& point,
const Cal3_S2::shared_ptr& cal) {
return Camera(pose, cal).project2(point);
}
/* ************************************************************************* */
TEST( PinholePose, Dproject)
{
Matrix Dpose, Dpoint;
Point2 result = camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
EXPECT(assert_equal(Point2(-100, 100), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point);
}
/* ************************************************************************* */
TEST( PinholePose, Dproject2)
{
Matrix Dcamera, Dpoint;
Point2 result = camera.project2(point1, Dcamera, Dpoint);
Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
EXPECT(assert_equal(result, Point2(-100, 100) ));
EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);
}
/* ************************************************************************* */
TEST( PinholePose, range0) {
Matrix D1; Matrix D2;
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static double range1(const Camera& camera, const Pose3& pose) {
return camera.range(pose);
}
/* ************************************************************************* */
TEST( PinholePose, range1) {
Matrix D1; Matrix D2;
double result = camera.range(pose1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
typedef PinholePose<Cal3Bundler> Camera2;
static const boost::shared_ptr<Cal3Bundler> K2 =
boost::make_shared<Cal3Bundler>(625, 1e-3, 1e-3);
static const Camera2 camera2(pose1, K2);
static double range2(const Camera& camera, const Camera2& camera2) {
return camera.range<Cal3Bundler>(camera2);
}
/* ************************************************************************* */
TEST( PinholePose, range2) {
Matrix D1; Matrix D2;
double result = camera.range<Cal3Bundler>(camera2, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static const CalibratedCamera camera3(pose1);
static double range3(const Camera& camera, const CalibratedCamera& camera3) {
return camera.range(camera3);
}
/* ************************************************************************* */
TEST( PinholePose, range3) {
Matrix D1; Matrix D2;
double result = camera.range(camera3, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -16,8 +16,6 @@
* @date Feb 7, 2012
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/EssentialMatrix.h>

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

@ -15,13 +15,13 @@
* @brief test SimpleCamera class
*/
#include <cmath>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;

1
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>

1
gtsam/slam/expressions.h
View File

@ -8,6 +8,7 @@
#pragma once
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>

14
gtsam/slam/tests/testDataset.cpp
View File

@ -15,16 +15,16 @@
* @author Richard Roberts, Luca Carlone
*/
#include <CppUnitLite/TestHarness.h>
#include <boost/algorithm/string.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/algorithm/string.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam::symbol_shorthand;
using namespace std;

1
gtsam/slam/tests/testLago.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>

3
tests/testManifold.cpp
View File

@ -17,9 +17,8 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/VectorSpace.h>