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

Merged in feature/SmartFactors3 (pull request #124) 

SmartFactors refactor phase 3
release/4.3a0
Frank Dellaert 2015-06-24 21:39:04 -07:00
parent 9404a49d42 d415cffd4b
commit a933b404f3
52 changed files with 3985 additions and 2955 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cmake/GtsamTesting.cmake
View File

@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
add_dependencies(all.tests ${script_name})
if(NOT XCODE_VERSION)
add_dependencies(all.tests ${script_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

1
examples/Data/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.txt

17
examples/SFMExample_SmartFactor.cpp
View File

@ -34,6 +34,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -55,12 +58,12 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
@ -68,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise, K);
smartfactor->add(measurement, i, measurementNoise);
}
// insert the smart factor in the graph
@ -77,15 +80,15 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
graph.print("Factor Graph:\n");
@ -94,7 +97,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results

17
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -30,6 +30,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -51,16 +54,16 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i, measurementNoise, K);
smartfactor->add(measurement, i, measurementNoise);
}
// insert the smart factor in the graph
@ -69,18 +72,18 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.

33
gtsam.h
View File

@ -812,7 +812,7 @@ class CalibratedCamera {
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
// Standard Interface
gtsam::Pose3 pose() const;
@ -848,7 +848,7 @@ class PinholeCamera {
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@ -884,7 +884,7 @@ virtual class SimpleCamera {
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@ -2352,18 +2352,31 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/slam/SmartProjectionFactor.h>
class SmartProjectionParams {
SmartProjectionParams();
// TODO(frank): make these work:
// void setLinearizationMode(LinearizationMode linMode);
// void setDegeneracyMode(DegeneracyMode degMode);
void setRankTolerance(double rankTol);
void setEnableEPI(bool enableEPI);
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(double rankTol);
SmartProjectionPoseFactor();
SmartProjectionPoseFactor(const CALIBRATION* K);
SmartProjectionPoseFactor(const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
const gtsam::noiseModel::Base* noise_i);
// enabling serialization functionality
//void serialize() const;

47
gtsam/geometry/CalibratedCamera.cpp
View File

@ -85,8 +85,7 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
}
/* ************************************************************************* */
Point2 PinholeBase::project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint) {
Point2 PinholeBase::Project(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)
@ -94,10 +93,22 @@ Point2 PinholeBase::project_to_camera(const Point3& pc,
return Point2(u, v);
}
/* ************************************************************************* */
Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
if (Dpoint) {
Matrix32 Dpoint3_pc;
Matrix23 Duv_point3;
Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
*Dpoint = Duv_point3 * Dpoint3_pc;
return uv;
} else
return Project(pc.point3());
}
/* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
const Point2 pn = Project(pc);
return make_pair(pn, pc.z() > 0);
}
@ -109,9 +120,9 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
const Point2 pn = Project(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
@ -123,6 +134,32 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
return pn;
}
/* ************************************************************************* */
Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
// world to camera coordinate
Matrix23 Dpc_rot;
Matrix2 Dpc_point;
const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
Dpose ? &Dpc_point : 0);
// camera to normalized image coordinate
Matrix2 Dpn_pc;
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
// chain the Jacobian matrices
if (Dpose) {
// only rotation is important
Matrix26 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
*Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6
}
if (Dpoint)
*Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {

59
gtsam/geometry/CalibratedCamera.h
View File

@ -44,6 +44,18 @@ public:
*/
class GTSAM_EXPORT PinholeBase {
public:
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
Pose3 pose_; ///< 3D pose of camera
@ -135,6 +147,16 @@ public:
return pose_;
}
/// get rotation
const Rot3& rotation() const {
return pose_.rotation();
}
/// get translation
const Point3& translation() const {
return pose_.translation();
}
/// return pose, with derivative
const Pose3& getPose(OptionalJacobian<6, 6> H) const;
@ -147,14 +169,21 @@ public:
* 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, //
static Point2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/**
* Project from 3D point at infinity 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(const Unit3& pc, //
OptionalJacobian<2, 2> 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
/** 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
@ -162,9 +191,33 @@ public:
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point at infinity 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 Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> 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);
/// @}
/// @name Advanced interface
/// @{
/**
* Return the start and end indices (inclusive) of the translation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
/// @}
private:
/** Serialization function */

266
gtsam/geometry/CameraSet.h
View File

@ -21,13 +21,14 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/base/Testable.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastMap.h>
#include <vector>
namespace gtsam {
/**
* @brief A set of cameras, all with their own calibration
* Assumes that a camera is laid out as 6 Pose3 parameters then calibration
*/
template<class CAMERA>
class CameraSet: public std::vector<CAMERA> {
@ -40,28 +41,46 @@ protected:
*/
typedef typename CAMERA::Measurement Z;
static const int D = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
/// Make a vector of re-projection errors
static Vector ErrorVector(const std::vector<Z>& predicted,
const std::vector<Z>& measured) {
// Check size
size_t m = predicted.size();
if (measured.size() != m)
throw std::runtime_error("CameraSet::errors: size mismatch");
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
}
return b;
}
public:
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::pair<Key, MatrixZD> FBlock; // Fblocks
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD> FBlocks;
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "") const {
virtual void print(const std::string& s = "") const {
std::cout << s << "CameraSet, cameras = \n";
for (size_t k = 0; k < this->size(); ++k)
this->at(k).print();
this->at(k).print(s);
}
/// equals
virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
bool equals(const CameraSet& p, double tol = 1e-9) const {
if (this->size() != p.size())
return false;
bool camerasAreEqual = true;
@ -74,31 +93,227 @@ public:
}
/**
* Project a point, with derivatives in this, point, and calibration
* Project a point (possibly Unit3 at infinity), with derivatives
* Note that F is a sparse block-diagonal matrix, so instead of a large dense
* matrix this function returns the diagonal blocks.
* throws CheiralityException
*/
std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
boost::none, boost::optional<Matrix&> E = boost::none,
boost::optional<Matrix&> H = boost::none) const {
template<class POINT>
std::vector<Z> project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
size_t nrCameras = this->size();
if (F) F->resize(ZDim * nrCameras, 6);
if (E) E->resize(ZDim * nrCameras, 3);
if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
std::vector<Z> z(nrCameras);
static const int N = FixedDimension<POINT>::value;
for (size_t i = 0; i < nrCameras; i++) {
Eigen::Matrix<double, ZDim, 6> Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, Dim - 6> Hi;
z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
// Allocate derivatives
if (E)
E->resize(ZDim * m, N);
if (Fs)
Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, N>(ZDim * i, 0) = Ei;
}
return z;
}
/// Calculate vector [project2(point)-z] of re-projection errors
template<class POINT>
Vector reprojectionError(const POINT& point, const std::vector<Z>& measured,
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian(i, m) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i, i) = Fi.transpose()
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi);
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i, j) = -Fi.transpose()
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(m, m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/// Computes Point Covariance P, with lambda parameter
template<int N> // N = 2 or 3
static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
const Matrix& E, double lambda, bool diagonalDamping = false) {
Matrix EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian
EtE.diagonal() += lambda * EtE.diagonal();
} else {
DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n, n);
}
P = (EtE).inverse();
}
/// Computes Point Covariance P, with lambda parameter, dynamic version
static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
bool diagonalDamping = false) {
if (E.cols() == 2) {
Matrix2 P2;
ComputePointCovariance(P2, E, lambda, diagonalDamping);
return P2;
} else {
Matrix3 P3;
ComputePointCovariance(P3, E, lambda, diagonalDamping);
return P3;
}
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* Dynamic version
*/
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
const Matrix& E, const Vector& b, const double lambda = 0.0,
bool diagonalDamping = false) {
SymmetricBlockMatrix augmentedHessian;
if (E.cols() == 2) {
Matrix2 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
} else {
Matrix3 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
}
return augmentedHessian;
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
template<int N> // N = 2 or 3
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
const Eigen::Matrix<double, N, N>& P, const Vector& b,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
assert(keys.size()==Fs.size());
assert(keys.size()<=allKeys.size());
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
Eigen::Matrix<double, D, D> matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
// a single point is observed in m cameras
size_t m = Fs.size(); // cameras observing current point
size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
assert(allKeys.size()==M);
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera in the current factor
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = E.template block<ZDim, N>(
ZDim * i, 0) * P;
// D = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap.at(keys[i]);
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal()
+ Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i, aug_i);
// add contribution of current factor
augmentedHessian(aug_i, aug_i) = matrixBlock
+ (Fi.transpose()
* (Fi - Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
DenseIndex aug_j = KeySlotMap.at(keys[j]);
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_j) =
augmentedHessian(aug_i, aug_j).knownOffDiagonal()
- Fi.transpose()
* (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(M, M)(0, 0) += b.squaredNorm();
}
private:
/// Serialization function
@ -109,6 +324,9 @@ private:
}
};
template<class CAMERA>
const int CameraSet<CAMERA>::D;
template<class CAMERA>
const int CameraSet<CAMERA>::ZDim;

130
gtsam/geometry/PinholeCamera.h
View File

@ -31,14 +31,6 @@ namespace gtsam {
template<typename Calibration>
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
@ -153,7 +145,7 @@ public:
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
H->template block<6, 6>(0, 0) = I_6x6;
}
return Base::pose();
}
@ -184,16 +176,15 @@ public:
if ((size_t) d.size() == 6)
return PinholeCamera(this->pose().retract(d), calibration());
else
return PinholeCamera(this->pose().retract(d.head(6)),
return PinholeCamera(this->pose().retract(d.head<6>()),
calibration().retract(d.tail(calibration().dim())));
}
/// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = this->pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
d.template head<6>() = this->pose().localCoordinates(T2.pose());
d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
return d;
}
@ -208,101 +199,34 @@ public:
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
/** Templated projection of a 3D point or a point at infinity into the image
* @param pw either a Point3 or a Unit3, in world coordinates
*/
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = 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, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}
/** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
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 = 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 = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
return pi;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
template<class POINT>
Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
// We just call 3-derivative version in Base
Matrix26 Dpose;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
Dcamera || Dpoint ? &Dpi_pn : 0);
// If needed, calculate derivatives
Eigen::Matrix<double, 2, DimK> Dcal;
Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
Dcamera ? &Dcal : 0);
if (Dcamera)
*Dcamera << Dpi_pn * Dpose, Dcal;
if (Dpoint)
*Dpoint = Dpi_pn * (*Dpoint);
*Dcamera << Dpose, Dcal;
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/// project a point at infinity from world coordinates into the image
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
@ -350,7 +274,7 @@ public:
if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
Dother->block<1, 6>(0, 0) = Dother_;
}
return result;
}

123
gtsam/geometry/PinholePose.h
View File

@ -30,14 +30,19 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
template<typename CALIBRATION>
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
private:
public :
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
typedef Calibration CalibrationType;
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<CALIBRATION>::value;
public:
typedef CALIBRATION CalibrationType;
/// @name Standard Constructors
/// @{
@ -67,7 +72,7 @@ public :
}
/// return calibration
virtual const Calibration& calibration() const = 0;
virtual const CALIBRATION& calibration() const = 0;
/// @}
/// @name Transformations and measurement functions
@ -80,27 +85,65 @@ public :
return pn;
}
/** project a point from world coordinate to the image, fixed Jacobians
/** project a point from world coordinate to the image
* @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 {
Point2 project(const Point3& pw) const {
const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** project a point from world coordinate to the image
* @param pw is a point at infinity in the world coordinates
*/
Point2 project(const Unit3& pw) const {
const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** Templated projection of a point (possibly at infinity) from world coordinate to the image
* @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
template <class POINT>
Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
OptionalJacobian<2, DimK> Dcal) 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,
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose) *Dpose = Dpi_pn * (*Dpose);
if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// 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);
@ -108,9 +151,9 @@ public :
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Point3 backprojectPointAtInfinity(const Point2& p) const {
Unit3 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
const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose().rotation().rotate(pc);
}
@ -178,13 +221,13 @@ private:
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
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
typedef PinholeBaseK<CALIBRATION> Base; ///< base class has 3D pose as private member
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to fixed calibration
public:
@ -201,11 +244,11 @@ public:
/** constructor with pose, uses default calibration */
explicit PinholePose(const Pose3& pose) :
Base(pose), K_(new Calibration()) {
Base(pose), K_(new CALIBRATION()) {
}
/** constructor with pose and calibration */
PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
Base(pose), K_(K) {
}
@ -220,14 +263,14 @@ public:
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static PinholePose Level(const boost::shared_ptr<Calibration>& K,
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);
return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
}
/**
@ -240,8 +283,8 @@ public:
* @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>()) {
const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
boost::make_shared<CALIBRATION>()) {
return PinholePose(Base::LookatPose(eye, target, upVector), K);
}
@ -251,12 +294,12 @@ public:
/// Init from 6D vector
explicit PinholePose(const Vector &v) :
Base(v), K_(new Calibration()) {
Base(v), K_(new CALIBRATION()) {
}
/// Init from Vector and calibration
PinholePose(const Vector &v, const Vector &K) :
Base(v), K_(new Calibration(K)) {
Base(v), K_(new CALIBRATION(K)) {
}
/// @}
@ -286,10 +329,26 @@ public:
}
/// return calibration
virtual const Calibration& calibration() const {
virtual const CALIBRATION& calibration() const {
return *K_;
}
/** project a point from world coordinate to the image, 2 derivatives only
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
/// project2 version for point at infinity
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
/// @}
/// @name Manifold
/// @{
@ -336,14 +395,14 @@ private:
};
// end of class PinholePose
template<typename Calibration>
struct traits<PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
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> > {
template<typename CALIBRATION>
struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
PinholePose<CALIBRATION> > {
};
} // \ gtsam

85
gtsam/geometry/PinholeSet.h Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 PinholeSet.h
* @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
namespace gtsam {
/**
* PinholeSet: triangulates point and keeps an estimate of it around.
*/
template<class CAMERA>
class PinholeSet: public CameraSet<CAMERA> {
private:
typedef CameraSet<CAMERA> Base;
typedef PinholeSet<CAMERA> This;
protected:
public:
/** Virtual destructor */
virtual ~PinholeSet() {
}
/// @name Testable
/// @{
/// print
virtual void print(const std::string& s = "") const {
Base::print(s);
}
/// equals
bool equals(const PinholeSet& p, double tol = 1e-9) const {
return Base::equals(p, tol); // TODO all flags
}
/// @}
/// triangulateSafe
TriangulationResult triangulateSafe(
const std::vector<Point2>& measured,
const TriangulationParameters& params) const {
return gtsam::triangulateSafe(*this, measured, params);
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
template<class CAMERA>
struct traits<PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
};
template<class CAMERA>
struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
};
} // \ namespace gtsam

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

@ -88,13 +88,30 @@ TEST( CalibratedCamera, project)
}
/* ************************************************************************* */
TEST( CalibratedCamera, Dproject_to_camera1) {
Point3 pp(155,233,131);
Matrix actual;
CalibratedCamera::project_to_camera(pp, actual);
Matrix expected_numerical = numericalDerivative11<Point2,Point3>(
boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
CHECK(assert_equal(expected_numerical, actual));
static Point2 Project1(const Point3& point) {
return PinholeBase::Project(point);
}
TEST( CalibratedCamera, DProject1) {
Point3 pp(155, 233, 131);
Matrix test1;
CalibratedCamera::Project(pp, test1);
Matrix test2 = numericalDerivative11<Point2, Point3>(Project1, pp);
CHECK(assert_equal(test1, test2));
}
/* ************************************************************************* */
static Point2 Project2(const Unit3& point) {
return PinholeBase::Project(point);
}
Unit3 pointAtInfinity(0, 0, 1000);
TEST( CalibratedCamera, DProjectInfinity) {
Matrix test1;
CalibratedCamera::Project(pointAtInfinity, test1);
Matrix test2 = numericalDerivative11<Point2, Unit3>(Project2,
pointAtInfinity);
CHECK(assert_equal(test1, test2));
}
/* ************************************************************************* */
@ -128,6 +145,36 @@ TEST( CalibratedCamera, Dproject_point_pose2)
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) {
return camera.project2(point);
}
TEST( CalibratedCamera, Dproject_point_pose_infinity)
{
Matrix Dpose, Dpoint;
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
CHECK(assert_equal(Point2(), 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_infinity)
{
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.project2(pointAtInfinity, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

125
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -31,43 +31,105 @@ using namespace gtsam;
#include <gtsam/geometry/Cal3Bundler.h>
TEST(CameraSet, Pinhole) {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set;
typedef vector<Point2> ZZ;
CameraSet<Camera> set;
Set set;
Camera camera;
set.push_back(camera);
set.push_back(camera);
Point3 p(0, 0, 1);
CHECK(assert_equal(set, set));
CameraSet<Camera> set2 = set;
EXPECT(assert_equal(set, set));
Set set2 = set;
set2.push_back(camera);
CHECK(!set.equals(set2));
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix46 actualF;
Matrix43 actualE;
Matrix43 actualH;
Matrix actualE;
Matrix29 F1;
{
Matrix26 F1;
Matrix23 E1;
Matrix23 H1;
camera.project(p, F1, E1, H1);
camera.project2(p, F1, E1);
actualE.resize(4,3);
actualE << E1, E1;
actualF << F1, F1;
actualH << H1, H1;
}
// Check computed derivatives
Matrix F, E, H;
set.project(p, F, E, H);
CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E));
CHECK(assert_equal(actualH, H));
Set::FBlocks Fs;
Matrix E;
set.project2(p, Fs, E);
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
// Check errors
ZZ measured;
measured.push_back(Point2(1, 2));
measured.push_back(Point2(3, 4));
Vector4 expectedV;
// reprojectionError
expectedV << -1, -2, -3, -4;
Vector actualV = set.reprojectionError(p, measured);
EXPECT(assert_equal(expectedV, actualV));
// Check Schur complement
Matrix F(4, 18);
F << F1, Matrix29::Zero(), Matrix29::Zero(), F1;
Matrix Ft = F.transpose();
Matrix34 Et = E.transpose();
Matrix3 P = Et * E;
Matrix schur(19, 19);
Vector4 b = actualV;
Vector v = Ft * (b - E * P * Et * b);
schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30;
SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b);
EXPECT(assert_equal(schur, actualReduced.matrix()));
// Check Schur complement update, same order, should just double
FastVector<Key> allKeys, keys;
allKeys.push_back(1);
allKeys.push_back(2);
keys.push_back(1);
keys.push_back(2);
Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced);
EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix()));
// Check Schur complement update, keys reversed
FastVector<Key> keys2;
keys2.push_back(2);
keys2.push_back(1);
Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
Vector4 reverse_b;
reverse_b << b.tail<2>(), b.head<2>();
Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
Matrix A(19, 19);
A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30;
EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix()));
// reprojectionErrorAtInfinity
Unit3 pointAtInfinity(0, 0, 1000);
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(expectedV, actualV));
LONGS_EQUAL(2, Fs.size());
{
Matrix22 E1;
camera.project2(pointAtInfinity, F1, E1);
actualE.resize(4,2);
actualE << E1, E1;
}
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */
@ -83,26 +145,27 @@ TEST(CameraSet, Stereo) {
// Check measurements
StereoPoint2 expected(0, -1, 0);
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix66 actualF;
Matrix63 actualE;
Matrix F1;
{
Matrix36 F1;
Matrix33 E1;
camera.project(p, F1, E1);
camera.project2(p, F1, E1);
actualE << E1, E1;
actualF << F1, F1;
}
// Check computed derivatives
Matrix F, E;
set.project(p, F, E);
CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E));
CameraSet<StereoCamera>::FBlocks Fs;
Matrix E;
set.project2(p, Fs, E);
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */

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

@ -45,10 +45,10 @@ 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);
static const Unit3 point1_inf(-0.16,-0.16, -1.0);
static const Unit3 point2_inf(-0.16, 0.16, -1.0);
static const Unit3 point3_inf( 0.16, 0.16, -1.0);
static const Unit3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholeCamera, constructor)
@ -154,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 1., 0.);
Point2 x = camera.projectPointAtInfinity(expected);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 expected(0., 1., 0.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
@ -169,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3)
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 0., 1.);
Point2 x = camera.projectPointAtInfinity(expected);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 expected(0., 0., 1.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
@ -197,17 +197,17 @@ TEST( PinholeCamera, Dproject)
}
/* ************************************************************************* */
static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
return Camera(pose,cal).projectPointAtInfinity(point3D);
static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) {
return Camera(pose,cal).project(point3D);
}
TEST( PinholeCamera, Dproject_Infinity)
{
Matrix Dpose, Dpoint, Dcal;
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
// test Projection
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal);
Point2 expected(-5.0, 5.0);
EXPECT(assert_equal(actual, expected, 1e-7));

79
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -46,10 +46,10 @@ 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);
static const Unit3 point1_inf(-0.16,-0.16, -1.0);
static const Unit3 point2_inf(-0.16, 0.16, -1.0);
static const Unit3 point3_inf( 0.16, 0.16, -1.0);
static const Unit3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholePose, constructor)
@ -144,11 +144,11 @@ 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);
Matrix expectedDcamera = numericalDerivative31(project3, pose, point1, K);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7));
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
}
/* ************************************************************************* */
@ -161,11 +161,11 @@ 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);
Matrix expectedDcamera = numericalDerivative21(project4, camera, point1);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7));
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
}
/* ************************************************************************* */
@ -176,12 +176,31 @@ TEST( CalibratedCamera, Dproject3)
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix expectedDcamera = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(expectedDcamera, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
const Cal3_S2::shared_ptr& cal) {
return Camera(pose, cal).project(pointAtInfinity);
}
/* ************************************************************************* */
TEST( PinholePose, DprojectAtInfinity2)
{
Unit3 pointAtInfinity(0,0,1000);
Matrix Dpose, Dpoint;
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K);
Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K);
EXPECT(assert_equal(Point2(0,0), result));
EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7));
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);
@ -191,12 +210,12 @@ static double range0(const Camera& camera, const Point3& 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);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}
/* ************************************************************************* */
@ -208,11 +227,11 @@ static double range1(const Camera& camera, const Pose3& 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);
Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}
/* ************************************************************************* */
@ -228,11 +247,11 @@ static double range2(const Camera& camera, const Camera2& 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);
Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}
/* ************************************************************************* */
@ -245,11 +264,11 @@ static double range3(const Camera& camera, const CalibratedCamera& 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);
Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3);
Matrix expectedDpoint = 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));
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}
/* ************************************************************************* */

158
gtsam/geometry/tests/testPinholeSet.cpp Normal file
View File

@ -0,0 +1,158 @@
/* ----------------------------------------------------------------------------
* 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 testCameraSet.cpp
* @brief Unit tests for testCameraSet Class
* @author Frank Dellaert
* @date Feb 19, 2015
*/
#include <gtsam/geometry/PinholeSet.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
#include <gtsam/geometry/CalibratedCamera.h>
TEST(PinholeSet, Stereo) {
typedef vector<Point2> ZZ;
PinholeSet<CalibratedCamera> set;
CalibratedCamera camera;
set.push_back(camera);
set.push_back(camera);
// set.print("set: ");
Point3 p(0, 0, 1);
EXPECT_LONGS_EQUAL(6, traits<CalibratedCamera>::dimension);
// Check measurements
Point2 expected(0, 0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix43 actualE;
Matrix F1;
{
Matrix23 E1;
camera.project2(p, F1, E1);
actualE << E1, E1;
}
// Check computed derivatives
PinholeSet<CalibratedCamera>::FBlocks Fs;
Matrix E;
set.project2(p, Fs, E);
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
EXPECT(assert_equal(actualE, E));
// Instantiate triangulateSafe
// TODO triangulation does not work yet for CalibratedCamera
// PinholeSet<CalibratedCamera>::Result actual = set.triangulateSafe(z);
}
/* ************************************************************************* */
// Cal3Bundler test
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
TEST(PinholeSet, Pinhole) {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef vector<Point2> ZZ;
PinholeSet<Camera> set;
Camera camera;
set.push_back(camera);
set.push_back(camera);
Point3 p(0, 0, 1);
EXPECT(assert_equal(set, set));
PinholeSet<Camera> set2 = set;
set2.push_back(camera);
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix actualE;
Matrix F1;
{
Matrix23 E1;
camera.project2(p, F1, E1);
actualE.resize(4, 3);
actualE << E1, E1;
}
// Check computed derivatives
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;
set.project2(p, Fs, E);
EXPECT(assert_equal(actualE, E));
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
}
// Check errors
ZZ measured;
measured.push_back(Point2(1, 2));
measured.push_back(Point2(3, 4));
Vector4 expectedV;
// reprojectionError
expectedV << -1, -2, -3, -4;
Vector actualV = set.reprojectionError(p, measured);
EXPECT(assert_equal(expectedV, actualV));
// reprojectionErrorAtInfinity
Unit3 pointAtInfinity(0, 0, 1000);
{
Matrix22 E1;
camera.project2(pointAtInfinity, F1, E1);
actualE.resize(4, 2);
actualE << E1, E1;
}
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(actualE, E));
LONGS_EQUAL(2, Fs.size());
EXPECT(assert_equal(F1, Fs[0]));
EXPECT(assert_equal(F1, Fs[1]));
}
EXPECT(assert_equal(expectedV, actualV));
// Instantiate triangulateSafe
TriangulationParameters params;
TriangulationResult actual = set.triangulateSafe(z, params);
CHECK(actual.degenerate());
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

93
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <CppUnitLite/TestHarness.h>
@ -49,6 +50,7 @@ Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
//******************************************************************************
// Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses) {
vector<Pose3> poses;
@ -57,24 +59,37 @@ TEST( triangulation, twoPoses) {
poses += pose1, pose2;
measurements += z1, z2;
bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 1. Test simple DLT, perfect in no noise situation
bool optimize = false;
boost::optional<Point3> actual1 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 4. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
}
//******************************************************************************
// Similar, but now with Bundler calibration
TEST( triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = //
@ -95,17 +110,17 @@ TEST( triangulation, twoPosesBundler) {
bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
boost::optional<Point3> actual = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual, 1e-7));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
// Add some noise and try again
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
}
//******************************************************************************
@ -116,17 +131,17 @@ TEST( triangulation, fourPoses) {
poses += pose1, pose2;
measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = //
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@ -150,7 +165,7 @@ TEST( triangulation, fourPoses) {
SimpleCamera camera4(pose4, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
poses += pose4;
measurements += Point2(400, 400);
@ -180,17 +195,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera1, camera2;
measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = //
boost::optional<Point3> actual = //
triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = //
boost::optional<Point3> actual2 = //
triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@ -216,7 +231,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
SimpleCamera camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
cameras += camera4;
measurements += Point2(400, 400);
@ -244,23 +259,19 @@ TEST( triangulation, twoIdenticalPoses) {
}
//******************************************************************************
/*
TEST( triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation
TEST( triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation
Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480);
vector<Pose3> poses;
vector<Point2> measurements;
vector<Pose3> poses;
vector<Point2> measurements;
poses += Pose3();
measurements += Point2();
poses += Pose3();
measurements += Point2();
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal),
TriangulationUnderconstrainedException);
}
*/
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException);
}
//******************************************************************************
int main() {

1
gtsam/geometry/triangulation.cpp
View File

@ -46,7 +46,6 @@ Vector4 triangulateHomogeneousDLT(
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol);
// std::cout << "s " << s.transpose() << std:endl;
if (rank < 3)
throw(TriangulationUnderconstrainedException());

208
gtsam/geometry/triangulation.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -64,7 +65,6 @@ GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol = 1e-9);
///
/**
* Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses
@ -86,8 +86,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i];
PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
graph.push_back(TriangulationFactor<CALIBRATION> //
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.push_back(TriangulationFactor<Camera> //
(camera_i, measurements[i], unit2, landmarkKey));
}
return std::make_pair(graph, values);
@ -114,13 +115,22 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<typename CAMERA::CalibrationType> //
graph.push_back(TriangulationFactor<CAMERA> //
(camera_i, measurements[i], unit2, landmarkKey));
}
return std::make_pair(graph, values);
}
///
/// PinholeCamera specific version
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey,
const Point3& initialEstimate) {
return triangulationGraph<PinholeCamera<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate);
}
/**
* Optimize for triangulation
* @param graph nonlinear factors for projection
@ -147,8 +157,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements,
Symbol('p', 0), initialEstimate);
boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0));
}
@ -168,12 +178,21 @@ Point3 triangulateNonlinear(
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(cameras, measurements,
Symbol('p', 0), initialEstimate);
boost::tie(graph, values) = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0));
}
/// PinholeCamera specific version
template<class CALIBRATION>
Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
(cameras, measurements, initialEstimate);
}
/**
* Create a 3*4 camera projection matrix from calibration and pose.
* Functor for partial application on calibration
@ -224,12 +243,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization
// Then refine using non-linear optimization
if (optimize)
point = triangulateNonlinear(poses, sharedCal, measurements, point);
point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras
// verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(point);
if (p_local.z() <= 0)
@ -265,9 +285,8 @@ Point3 triangulatePoint3(
throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration
typedef PinholeCamera<typename CAMERA::CalibrationType> Camera;
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras)
BOOST_FOREACH(const CAMERA& camera, cameras)
projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose()));
@ -275,11 +294,11 @@ Point3 triangulatePoint3(
// The n refine using non-linear optimization
if (optimize)
point = triangulateNonlinear(cameras, measurements, point);
point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Camera& camera, cameras) {
// verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const CAMERA& camera, cameras) {
const Point3& p_local = camera.pose().transform_to(point);
if (p_local.z() <= 0)
throw(TriangulationCheiralityException());
@ -289,5 +308,160 @@ Point3 triangulatePoint3(
return point;
}
/// Pinhole-specific version
template<class CALIBRATION>
Point3 triangulatePoint3(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize);
}
struct TriangulationParameters {
double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
bool enableEPI; ///< if set to true, will refine triangulation using LM
/**
* if the landmark is triangulated at distance larger than this,
* result is flagged as degenerate.
*/
double landmarkDistanceThreshold; //
/**
* If this is nonnegative the we will check if the average reprojection error
* is smaller than this threshold after triangulation, otherwise result is
* flagged as degenerate.
*/
double dynamicOutlierRejectionThreshold;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param enableEPI if true refine triangulation with embedded LM iterations
* @param landmarkDistanceThreshold flag as degenerate if point further than this
* @param dynamicOutlierRejectionThreshold or if average error larger than this
*
*/
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationParameters& p) {
os << "rankTolerance = " << p.rankTolerance << std::endl;
os << "enableEPI = " << p.enableEPI << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
return os;
}
};
/**
* TriangulationResult is an optional point, along with the reasons why it is invalid.
*/
class TriangulationResult: public boost::optional<Point3> {
enum Status {
VALID, DEGENERATE, BEHIND_CAMERA
};
Status status_;
TriangulationResult(Status s) :
status_(s) {
}
public:
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);
}
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA);
}
bool degenerate() const {
return status_ == DEGENERATE;
}
bool behindCamera() const {
return status_ == BEHIND_CAMERA;
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationResult& result) {
if (result)
os << "point = " << *result << std::endl;
else
os << "no point, status = " << result.status_ << std::endl;
return os;
}
};
/// triangulateSafe: extensive checking of the outcome
template<class CAMERA>
TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measured,
const TriangulationParameters& params) {
size_t m = cameras.size();
// if we have a single pose the corresponding factor is uninformative
if (m < 2)
return TriangulationResult::Degenerate();
else
// We triangulate the 3D position of the landmark
try {
Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI);
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;
double totalReprojError = 0.0;
BOOST_FOREACH(const CAMERA& camera, cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& pose.translation().distance(point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
// Only needed if this was not yet handled by exception
const Point3& p_local = pose.transform_to(point);
if (p_local.z() <= 0)
return TriangulationResult::BehindCamera();
#endif
// Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm();
}
i += 1;
}
// Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate();
// all good!
return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) {
// This exception is thrown if
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
return TriangulationResult::BehindCamera();
}
}
} // \namespace gtsam

8
gtsam/linear/NoiseModel.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
@ -506,7 +507,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar
/* ************************************************************************* */
void Isotropic::print(const string& name) const {
cout << name << "isotropic sigma " << " " << sigma_ << endl;
cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl;
}
/* ************************************************************************* */
@ -534,6 +535,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
H *= invsigma_;
}
/* ************************************************************************* */
void Isotropic::whitenInPlace(Vector& v) const {
v *= invsigma_;
}
/* ************************************************************************* */
void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
H *= invsigma_;

1
gtsam/linear/NoiseModel.h
View File

@ -550,6 +550,7 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void whitenInPlace(Vector& v) const;
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
/**

2
gtsam/slam/RegularHessianFactor.h → gtsam/linear/RegularHessianFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <boost/foreach.hpp>
#include <vector>

0
gtsam/slam/RegularJacobianFactor.h → gtsam/linear/RegularJacobianFactor.h
View File

12
gtsam/linear/VectorValues.cpp
View File

@ -66,6 +66,18 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
// Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
// it is inserted into the values_ map.
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
throw std::invalid_argument(
"Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
+ "' already in this VectorValues.");
return result.first;
}
/* ************************************************************************* */
void VectorValues::update(const VectorValues& values)
{

13
gtsam/linear/VectorValues.h
View File

@ -181,23 +181,14 @@ namespace gtsam {
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator insert(Key j, const Vector& value) {
return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector
return insert(std::make_pair(j, value));
}
/** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c
* j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator insert(const std::pair<Key, Vector>& key_value) {
// Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
// it is inserted into the values_ map.
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
throw std::invalid_argument(
"Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
+ "' already in this VectorValues.");
return result.first;
}
iterator insert(const std::pair<Key, Vector>& key_value);
/** Insert all values from \c values. Throws an invalid_argument exception if any keys to be
* inserted are already used. */

2
gtsam/slam/tests/testRegularHessianFactor.cpp → gtsam/linear/tests/testRegularHessianFactor.cpp
View File

@ -15,7 +15,7 @@
* @date March 4, 2014
*/
#include <gtsam/slam/RegularHessianFactor.h>
#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

2
gtsam/slam/tests/testRegularJacobianFactor.cpp → gtsam/linear/tests/testRegularJacobianFactor.cpp
View File

@ -16,7 +16,7 @@
* @date Nov 12, 2014
*/
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>

3
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -175,7 +175,8 @@ using namespace binary;
Expression<Cal3_S2> K(3);
// Create expression tree
Expression<Point2> projection(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
Expression<Point2> projection(f, p_cam);
Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
}
/* ************************************************************************* */

4
gtsam/slam/EssentialMatrixFactor.h
View File

@ -173,7 +173,7 @@ public:
Point3 _1T2 = E.direction().point3();
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
pn = SimpleCamera::project_to_camera(dP2);
pn = PinholeBase::Project(dP2);
} else {
@ -186,7 +186,7 @@ public:
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Matrix23 Dpn_dP2;
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
pn = PinholeBase::Project(dP2, Dpn_dP2);
if (DE) {
Matrix DdP2_E(3, 5);

20
gtsam/slam/JacobianFactorQ.h
View File

@ -17,7 +17,7 @@
#pragma once
#include "RegularJacobianFactor.h"
#include <gtsam/linear/RegularJacobianFactor.h>
namespace gtsam {
/**
@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
@ -42,7 +42,6 @@ public:
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
@ -51,24 +50,25 @@ public:
}
/// Constructor
JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, const SharedDiagonal& model =
SharedDiagonal()) :
JacobianFactorQ(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
// TODO: can we do better ?
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF;
QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k];
QF.push_back(
KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
}
// Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model);
JacobianFactor::fillTerms(QF, - Q * b, model);
}
};
// end class JacobianFactorQ

17
gtsam/slam/JacobianFactorQR.h
View File

@ -6,8 +6,8 @@
*/
#pragma once
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam {
@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public:
/**
* Constructor
*/
JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, //
JacobianFactorQR(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model);
i += 1;
for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k];
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
b.segment < ZDim > (ZDim * k), model);
}
//gfg.print("gfg");

29
gtsam/slam/JacobianFactorSVD.h
View File

@ -5,18 +5,17 @@
*/
#pragma once
#include "gtsam/slam/RegularJacobianFactor.h"
#include <gtsam/linear/RegularJacobianFactor.h>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
* JacobianFactor for Schur complement
*/
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
@ -38,13 +37,21 @@ public:
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const Matrix& Enull, const Vector& b, //
/**
* @brief Constructor
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
* and a reduced point derivative, Enull
* and creates a reduced-rank Jacobian factor on the CameraSet
*
* @Fblocks:
*/
JacobianFactorSVD(const FastVector<Key>& keys,
const std::vector<MatrixZD>& Fblocks, const Matrix& Enull,
const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim * numKeys - 3;
size_t m2 = ZDim * numKeys - 3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
@ -52,10 +59,12 @@ public:
// JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
for (size_t k = 0; k < Fblocks.size(); ++k) {
Key key = keys[k];
QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
KeyMatrix(key,
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
}
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
}
};

229
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
@ -17,7 +18,7 @@ namespace gtsam {
/**
* RegularImplicitSchurFactor
*/
template<size_t D> //
template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor {
public:
@ -26,14 +27,20 @@ public:
protected:
typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
// This factor is closely related to a CameraSet
typedef CameraSet<CAMERA> Set;
std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
Vector b_; ///< 2m-dimensional RHS vector
typedef typename CAMERA::Measurement Z;
static const int D = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera)
const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector
public:
@ -41,54 +48,40 @@ public:
RegularImplicitSchurFactor() {
}
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys();
}
/// initialize keys from Fblocks
void initKeys() {
keys_.reserve(Fblocks_.size());
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_)
keys_.push_back(it.first);
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix& P,
const Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
}
/// Destructor
virtual ~RegularImplicitSchurFactor() {
}
// Write access, only use for construction!
inline std::vector<KeyMatrix2D>& Fblocks() {
return Fblocks_;
std::vector<MatrixZD>& FBlocks() const {
return FBlocks_;
}
inline Matrix3& PointCovariance() {
return PointCovariance_;
}
inline Matrix& E() {
const Matrix& E() const {
return E_;
}
inline Vector& b() {
const Vector& b() const {
return b_;
}
/// Get matrix P
inline const Matrix3& getPointCovariance() const {
const Matrix& getPointCovariance() const {
return PointCovariance_;
}
/// print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
for (size_t pos = 0; pos < size(); ++pos) {
std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl;
std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
}
std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
std::cout << "E:\n" << E_ << std::endl;
@ -100,10 +93,11 @@ public:
const This* f = dynamic_cast<const This*>(&lf);
if (!f)
return false;
for (size_t pos = 0; pos < size(); ++pos) {
if (keys_[pos] != f->keys_[pos]) return false;
if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
for (size_t k = 0; k < FBlocks_.size(); ++k) {
if (keys_[k] != f->keys_[k])
return false;
if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
return false;
}
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
&& equal_with_abs_tol(E_, f->E_, tol)
@ -126,18 +120,26 @@ public:
return Matrix();
}
virtual std::pair<Matrix, Vector> jacobian() const {
throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
throw std::runtime_error(
"RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector());
}
/// *Compute* full augmented information matrix
virtual Matrix augmentedInformation() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix();
// Do the Schur complement
SymmetricBlockMatrix augmentedHessian = //
Set::SchurComplement(FBlocks_, E_, b_);
return augmentedHessian.matrix();
}
/// *Compute* full information matrix
virtual Matrix information() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::information non implemented");
return Matrix();
Matrix augmented = augmentedInformation();
int m = this->keys_.size();
size_t M = D * m;
return augmented.block(0, 0, M, M);
}
/// Return the diagonal of the Hessian for this factor
@ -145,17 +147,17 @@ public:
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
VectorValues d;
for (size_t pos = 0; pos < size(); ++pos) { // for each camera
Key j = keys_[pos];
for (size_t k = 0; k < size(); ++k) { // for each camera
Key j = keys_[k];
// Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x 2) * (2 x 3)
const Matrix2D& Fj = Fblocks_[pos].second;
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
// D x 3 = (D x ZDim) * (ZDim x 3)
const MatrixZD& Fj = FBlocks_[k];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<ZDim, 3>(ZDim * k, 0);
Eigen::Matrix<double, D, 1> dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
// Vector column_k_Fj = Fj.col(k);
dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
// Vector column_k_FtE = FtE.row(k);
@ -181,13 +183,13 @@ public:
Key j = keys_[pos];
// Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x 2) * (2 x 3)
const Matrix2D& Fj = Fblocks_[pos].second;
// D x 3 = (D x ZDim) * (ZDim x 3)
const MatrixZD& Fj = FBlocks_[pos];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
* E_.block<ZDim, 3>(ZDim * pos, 0);
DVector dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
dj(k) = Fj.col(k).squaredNorm();
// (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
@ -202,38 +204,41 @@ public:
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
// F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
const Matrix2D& Fj = Fblocks_[pos].second;
// F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
const MatrixZD& Fj = FBlocks_[pos];
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<2, 3>(2 * pos, 0);
// * E_.block<ZDim, 3>(ZDim * pos, 0);
// blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
blocks[j] = Fj.transpose()
* (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
// Matrix2 Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj;
}
return blocks;
}
virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented");
}
virtual bool empty() const {
return false;
}
virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::negate non implemented");
}
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@ -251,22 +256,24 @@ public:
typedef std::vector<Vector2> Error2s;
/**
* @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
* @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
*/
void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
// d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2));
d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
* (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2;
e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
- E_.block<ZDim, 3>(ZDim * k, 0) * d2;
}
/*
@ -286,7 +293,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]);
e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError2(e1, e2);
double result = 0;
@ -308,7 +315,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
projectError(e1, e2);
double result = 0;
@ -321,21 +328,21 @@ public:
/**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/
void projectError(const Error2s& e1, Error2s& e2) const {
void projectError(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * e1 = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k];
// d1 = E.transpose() * e1 = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
}
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
}
/// Scratch space for multiplyHessianAdd
mutable Error2s e1, e2;
@ -356,19 +363,17 @@ public:
e2.resize(size());
// e1 = F * x = (2m*dm)*dm
size_t k = 0;
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
Key key = it.first;
e1[k++] = it.second * ConstDMap(x + D * key);
for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k];
e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
}
projectError(e1, e2);
// y += F.transpose()*e2 = (2d*2m)*2m
k = 0;
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
Key key = it.first;
DMap(y + D * key) += it.second.transpose() * alpha * e2[k++];
for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k];
DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
}
}
@ -389,7 +394,7 @@ public:
// e1 = F * x = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]);
e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError(e1, e2);
@ -401,8 +406,8 @@ public:
Vector& yi = it.first->second;
// Create the value as a zero vector if it does not exist.
if (it.second)
yi = Vector::Zero(Fblocks_[k].second.cols());
yi += Fblocks_[k].second.transpose() * alpha * e2[k];
yi = Vector::Zero(FBlocks_[k].cols());
yi += FBlocks_[k].transpose() * alpha * e2[k];
}
}
@ -412,9 +417,9 @@ public:
void multiplyHessianDummy(double alpha, const VectorValues& x,
VectorValues& y) const {
BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) {
for (size_t k = 0; k < size(); ++k) {
static const Vector empty;
Key key = Fi.first;
Key key = keys_[k];
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
Vector& yi = it.first->second;
yi = x.at(key);
@ -429,14 +434,14 @@ public:
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k);
e1[k] = b_.segment<ZDim>(ZDim * k);
projectError(e1, e2);
// g = F.transpose()*e2
VectorValues g;
for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k];
g.insert(key, -Fblocks_[k].second.transpose() * e2[k]);
g.insert(key, -FBlocks_[k].transpose() * e2[k]);
}
// return it
@ -456,27 +461,33 @@ public:
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k);
e1[k] = b_.segment<ZDim>(ZDim * k);
projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
Key j = keys_[k];
DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k];
DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
}
}
/// Gradient wrt a key at any values
Vector gradient(Key key, const VectorValues& x) const {
throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet");
throw std::runtime_error(
"gradient for RegularImplicitSchurFactor is not implemented yet");
}
};
// end class RegularImplicitSchurFactor
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::D;
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::ZDim;
// traits
template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
RegularImplicitSchurFactor<D> > {
template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
RegularImplicitSchurFactor<CAMERA> > {
};
}

736
gtsam/slam/SmartFactorBase.h
View File

@ -22,9 +22,9 @@
#include <gtsam/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorSVD.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/slam/RegularHessianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/geometry/CameraSet.h>
#include <boost/optional.hpp>
@ -41,13 +41,23 @@ namespace gtsam {
* The methods take a Cameras argument, which should behave like PinholeCamera, and
* the value of a point, which is kept in the base class.
*/
template<class CAMERA, size_t D>
template<class CAMERA>
class SmartFactorBase: public NonlinearFactor {
protected:
private:
typedef NonlinearFactor Base;
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
/**
* As of Feb 22, 2015, the noise model is the same for all measurements and
* is isotropic. This allows for moving most calculations of Schur complement
* etc to be moved to CameraSet very easily, and also agrees pragmatically
* with what is normally done.
*/
SharedIsotropic noiseModel_;
protected:
/**
* 2D measurement and noise model for each of the m views
* We keep a copy of measurements for I/O and computing the error.
@ -55,45 +65,54 @@ protected:
*/
std::vector<Z> measured_;
std::vector<SharedNoiseModel> noise_; ///< noise model used
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
/// @name Pose of the camera in the body frame
const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
/// @}
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, ZDim> MatrixD2; // F'
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
// Definitions for block matrices used internally
typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
/// shorthand for base class type
typedef NonlinearFactor Base;
/// shorthand for this class
typedef SmartFactorBase<CAMERA, D> This;
// check that noise model is isotropic and the same
// TODO, this is hacky, we should just do this via constructor, not add
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
if (!sharedNoiseModel)
return;
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
if (!noiseModel_)
noiseModel_ = sharedIsotropic;
else if (!sharedIsotropic->equals(*noiseModel_))
throw std::runtime_error(
"SmartFactorBase: cannot add measurements with different noise model");
}
public:
// Definitions for blocks of F, externally visible
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// We use the new CameraSte data structure to refer to a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Constructor
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
/// Constructor
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor) {
}
body_P_sensor_(body_P_sensor){}
/** Virtual destructor */
/// Virtual destructor, subclasses from NonlinearFactor
virtual ~SmartFactorBase() {
}
@ -101,36 +120,36 @@ public:
* Add a new measurement and pose key
* @param measured_i is the 2m dimensional projection of a single landmark
* @param poseKey is the index corresponding to the camera observing the landmark
* @param noise_i is the measurement noise
* @param sharedNoiseModel is the measurement noise
*/
void add(const Z& measured_i, const Key& poseKey_i,
const SharedNoiseModel& noise_i) {
void add(const Z& measured_i, const Key& cameraKey_i,
const SharedNoiseModel& sharedNoiseModel) {
this->measured_.push_back(measured_i);
this->keys_.push_back(poseKey_i);
this->noise_.push_back(noise_i);
this->keys_.push_back(cameraKey_i);
maybeSetNoiseModel(sharedNoiseModel);
}
/**
* Add a bunch of measurements, together with the camera keys and noises
*/
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
std::vector<SharedNoiseModel>& noises) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(poseKeys.at(i));
this->noise_.push_back(noises.at(i));
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noises.at(i));
}
}
/**
* Add a bunch of measurements and uses the same noise model for all of them
* Add a bunch of measurements and use the same noise model for all of them
*/
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(poseKeys.at(i));
this->noise_.push_back(noise);
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noise);
}
}
@ -143,7 +162,7 @@ public:
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
this->noise_.push_back(noise);
maybeSetNoiseModel(noise);
}
}
@ -157,9 +176,12 @@ public:
return measured_;
}
/** return the noise models */
const std::vector<SharedNoiseModel>& noise() const {
return noise_;
/// Collect all cameras: important that in key order
virtual Cameras cameras(const Values& values) const {
Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_)
cameras.push_back(values.at<CAMERA>(k));
return cameras;
}
/**
@ -172,11 +194,11 @@ public:
std::cout << s << "SmartFactorBase, z = \n";
for (size_t k = 0; k < measured_.size(); ++k) {
std::cout << "measurement, p = " << measured_[k] << "\t";
noise_[k]->print("noise model = ");
noiseModel_->print("noise model = ");
}
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
if(body_P_sensor_)
body_P_sensor_->print("body_P_sensor_:\n");
print("", keyFormatter);
}
/// equals
@ -189,518 +211,105 @@ public:
areMeasurementsEqual = false;
break;
}
return e && Base::equals(p, tol) && areMeasurementsEqual
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|| (body_P_sensor_ && e->body_P_sensor_
&& body_P_sensor_->equals(*e->body_P_sensor_)));
return e && Base::equals(p, tol) && areMeasurementsEqual;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
// Project into CameraSet
std::vector<Z> predicted;
try {
predicted = cameras.project(point);
} catch (CheiralityException&) {
std::cout << "reprojectionError: Cheirality exception " << std::endl;
exit(EXIT_FAILURE); // TODO: throw exception
/// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
template<class POINT>
Vector unwhitenedError(const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if(body_P_sensor_){
for(size_t i=0; i < Fs->size(); i++){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6);
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
Fs->at(i) = Fs->at(i) * J;
}
}
// Calculate vector of errors
size_t nrCameras = cameras.size();
Vector b(ZDim * nrCameras);
for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) {
Z e = predicted[i] - measured_.at(i);
b.segment<ZDim>(row) = e.vector();
}
return b;
}
/// Calculate vector of re-projection errors, noise model applied
Vector whitenedError(const Cameras& cameras, const Point3& point) const {
Vector b = reprojectionError(cameras, point);
size_t nrCameras = cameras.size();
for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim)
b.segment<ZDim>(row) = noise_.at(i)->whiten(b.segment<ZDim>(row));
return b;
return ue;
}
/**
* Calculate the error of the factor.
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
* Noise model applied
*/
template<class POINT>
Vector whitenedError(const Cameras& cameras, const POINT& point) const {
Vector e = cameras.reprojectionError(point, measured_);
if (noiseModel_)
noiseModel_->whitenInPlace(e);
return e;
}
/** Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
* This is different from reprojectionError(cameras,point) as each point is whitened
* Will be used in "error(Values)" function required by NonlinearFactor base class
*/
template<class POINT>
double totalReprojectionError(const Cameras& cameras,
const Point3& point) const {
Vector b = reprojectionError(cameras, point);
double overallError = 0;
size_t nrCameras = cameras.size();
for (size_t i = 0; i < nrCameras; i++)
overallError += noise_.at(i)->distance(b.segment<ZDim>(i * ZDim));
return 0.5 * overallError;
}
/**
* Compute whitenedError, returning only the derivative E, i.e.,
* the stacked ZDim*3 derivatives of project with respect to the point.
* Assumes non-degenerate ! TODO explain this
*/
Vector whitenedError(const Cameras& cameras, const Point3& point,
Matrix& E) const {
// Project into CameraSet, calculating derivatives
std::vector<Z> predicted;
try {
using boost::none;
predicted = cameras.project(point, none, E, none);
} catch (CheiralityException&) {
std::cout << "whitenedError(E): Cheirality exception " << std::endl;
exit(EXIT_FAILURE); // TODO: throw exception
}
// if needed, whiten
size_t m = keys_.size();
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Calculate error
const Z& zi = measured_.at(i);
Vector bi = (zi - predicted[i]).vector();
// if needed, whiten
SharedNoiseModel model = noise_.at(i);
if (model) {
// TODO: re-factor noiseModel to take any block/fixed vector/matrix
Vector dummy;
Matrix Ei = E.block<ZDim, 3>(row, 0);
model->WhitenSystem(Ei, dummy);
E.block<ZDim, 3>(row, 0) = Ei;
}
b.segment<ZDim>(row) = bi;
}
return b;
}
/**
* Compute F, E, and optionally H, where F and E are the stacked derivatives
* with respect to the cameras, point, and calibration, respectively.
* The value of cameras/point are passed as parameters.
* Returns error vector b
* TODO: the treatment of body_P_sensor_ is weird: the transformation
* is applied in the caller, but the derivatives are computed here.
*/
Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F,
Matrix& E, boost::optional<Matrix&> G = boost::none) const {
// Project into CameraSet, calculating derivatives
std::vector<Z> predicted;
try {
predicted = cameras.project(point, F, E, G);
} catch (CheiralityException&) {
std::cout << "whitenedError(E,F): Cheirality exception " << std::endl;
exit(EXIT_FAILURE); // TODO: throw exception
}
// Calculate error and whiten derivatives if needed
size_t m = keys_.size();
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Calculate error
const Z& zi = measured_.at(i);
Vector bi = (zi - predicted[i]).vector();
// if we have a sensor offset, correct camera derivatives
if (body_P_sensor_) {
// TODO: no simpler way ??
const Pose3& pose_i = cameras[i].pose();
Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
Matrix66 J;
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
F.block<ZDim, 6>(row, 0) *= J;
}
// if needed, whiten
SharedNoiseModel model = noise_.at(i);
if (model) {
// TODO, refactor noiseModel so we can take blocks
Matrix Fi = F.block<ZDim, 6>(row, 0);
Matrix Ei = E.block<ZDim, 3>(row, 0);
if (!G)
model->WhitenSystem(Fi, Ei, bi);
else {
Matrix Gi = G->block<ZDim, D - 6>(row, 0);
model->WhitenSystem(Fi, Ei, Gi, bi);
G->block<ZDim, D - 6>(row, 0) = Gi;
}
F.block<ZDim, 6>(row, 0) = Fi;
E.block<ZDim, 3>(row, 0) = Ei;
}
b.segment<ZDim>(row) = bi;
}
return b;
const POINT& point) const {
Vector e = whitenedError(cameras, point);
return 0.5 * e.dot(e);
}
/// Computes Point Covariance P from E
static Matrix3 PointCov(Matrix& E) {
static Matrix PointCov(Matrix& E) {
return (E.transpose() * E).inverse();
}
/// Computes Point Covariance P, with lambda parameter
static Matrix3 PointCov(Matrix& E, double lambda,
bool diagonalDamping = false) {
Matrix3 EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian
EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2);
} else {
EtE(0, 0) += lambda;
EtE(1, 1) += lambda;
EtE(2, 2) += lambda;
}
return (EtE).inverse();
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
const Point3& point) const {
whitenedError(cameras, point, E);
P = PointCov(E);
}
/**
* Compute F, E, and b (called below in both vanilla and SVD versions), where
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters.
* TODO: Kill this obsolete method
*/
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const {
template<class POINT>
void computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras, const POINT& point) const {
// Project into Camera set and calculate derivatives
// TODO: if D==6 we optimize only camera pose. That is fairly hacky!
Matrix F, G;
using boost::none;
boost::optional<Matrix&> optionalG(G);
b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG);
// Now calculate f and divide up the F derivatives into Fblocks
double f = 0.0;
size_t m = keys_.size();
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Accumulate normalized error
f += b.segment<ZDim>(row).squaredNorm();
// Get piece of F and possibly G
Matrix2D Fi;
if (D == 6)
Fi << F.block<ZDim, 6>(row, 0);
else
Fi << F.block<ZDim, 6>(row, 0), G.block<ZDim, D - 6>(row, 0);
// Push it onto Fblocks
Fblocks.push_back(KeyMatrix2D(keys_[i], Fi));
}
return f;
}
/// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks, Matrix& F) {
size_t m = Fblocks.size();
F.resize(ZDim * m, D * m);
F.setZero();
for (size_t i = 0; i < m; ++i)
F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second;
}
/**
* Compute F, E, and b, where F and E are the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters.
*/
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras, const Point3& point) const {
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, b, cameras, point);
FillDiagonalF(Fblocks, F);
return f;
// As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
// Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
// = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fblocks, E);
}
/// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point) const {
template<class POINT>
void computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const {
Matrix E;
double f = computeJacobians(Fblocks, E, b, cameras, point);
computeJacobians(Fblocks, E, b, cameras, point);
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
// Do SVD on A
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
size_t m = this->keys_.size();
// Enull = zeros(ZDim * m, ZDim * m - 3);
Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
return f;
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
}
/// Matrix version of SVD
// TODO, there should not be a Matrix version, really
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
const Cameras& cameras, const Point3& point) const {
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
FillDiagonalF(Fblocks, F);
return f;
}
/**
* Linearize returns a Hessianfactor that is an approximation of error(p)
*/
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
/// Linearize to a Hessianfactor
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
const Cameras& cameras, const Point3& point, const double lambda = 0.0,
bool diagonalDamping = false) const {
int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
std::vector<MatrixZD> Fblocks;
Matrix E;
Vector b;
double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
computeJacobians(Fblocks, E, b, cameras, point);
//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS
// Create structures for Hessian Factors
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
std::vector < Vector > gs(numKeys);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E,
b);
sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
// schurComplement(Fblocks, E, P, b, Gs, gs);
//std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
//std::vector < Vector > gs2(gs.begin(), gs.end());
return boost::make_shared < RegularHessianFactor<D> > (this->keys_, Gs, gs, f);
#else // we create directly a SymmetricBlockMatrix
size_t n1 = D * numKeys + 1;
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian);
#endif
}
/**
* Do Schur complement, given Jacobian as F,E,P.
* Slow version - works on full matrices
*/
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
// Schur complement trick
// Gs = F' * F - F' * E * inv(E'*E) * E' * F
// gs = F' * (b - E * inv(E'*E) * E' * b)
// This version uses full matrices
int numKeys = this->keys_.size();
/// Compute Full F ????
Matrix F;
FillDiagonalF(Fblocks, F);
Matrix H(D * numKeys, D * numKeys);
Vector gs_vector;
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment<D>(i1D);
for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
GsCount2++;
}
}
}
}
/**
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity
*/
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size();
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1, numKeys) = Fi1.transpose()
* b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
/**
* Do Schur complement, given Jacobian as F,E,P, return Gs/gs
* Fast version - works on with sparsity
*/
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size();
int GsIndex = 0;
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
// GsIndex points to the upper triangular blocks
// 0 1 2 3 4
// X 5 6 7 8
// X X 9 10 11
// X X X 12 13
// X X X X 14
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
{ // for i1 = i2
// D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
GsIndex++;
}
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
GsIndex++;
}
} // end of for over cameras
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// D = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i1 = this->keys_[i1];
DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor
augmentedHessian(aug_i1, aug_i1) =
matrixBlock
+ (Fi1.transpose()
* (Fi1
- Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1));
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_i2) =
augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
}
/**
@ -712,73 +321,100 @@ public:
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
// int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Vector b;
double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
std::vector<MatrixZD> Fblocks;
computeJacobians(Fblocks, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_,
augmentedHessian);
}
/**
* Return Jacobians as RegularImplicitSchurFactor with raw access
*/
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
f->initKeys();
return f;
/// Whiten the Jacobians computed by computeJacobians using noiseModel_
void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type
for (size_t i = 0; i < F.size(); i++)
F[i] = noiseModel_->Whiten(F[i]);
}
/// Return Jacobians as RegularImplicitSchurFactor with raw access
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E;
Vector b;
std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b);
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
P, b);
}
/**
* Return Jacobians as JacobianFactorQ
*/
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Vector b;
computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b);
std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point);
const size_t M = b.size();
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
}
/**
* Return Jacobians as JacobianFactor
* Return Jacobians as JacobianFactorSVD
* TODO lambda is currently ignored
*/
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
size_t m = this->keys_.size();
std::vector<MatrixZD> F;
Vector b;
Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
const size_t M = ZDim * m;
Matrix E0(M, M - 3);
computeJacobiansSVD(F, E0, b, cameras, point);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
noiseModel_->sigma());
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
}
/// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
size_t m = Fblocks.size();
F.resize(ZDim * m, Dim * m);
F.setZero();
for (size_t i = 0; i < m; ++i)
F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i);
}
Pose3 body_P_sensor() const{
if(body_P_sensor_)
return *body_P_sensor_;
else
return Pose3(); // if unspecified, the transformation is the identity
}
private:
/// Serialization function
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}
;
};
// end class SmartFactorBase
template<class CAMERA, size_t D>
const int SmartFactorBase<CAMERA, D>::ZDim;
// Definitions need to avoid link errors (above are only declarations)
template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
} // \ namespace gtsam

788
gtsam/slam/SmartProjectionFactor.h
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
@ -32,108 +31,135 @@
namespace gtsam {
/**
* Structure for storing some state memory, used to speed up optimization
* @addtogroup SLAM
*/
class SmartProjectionFactorState {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
protected:
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart projection factors
*/
class GTSAM_EXPORT SmartProjectionParams {
public:
SmartProjectionFactorState() {
}
// Hessian representation (after Schur complement)
bool calculatedHessian;
Matrix H;
Vector gs_vector;
std::vector<Matrix> Gs;
std::vector<Vector> gs;
double f;
};
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
/// @name Parameters governing the triangulation
/// @{
mutable TriangulationParameters triangulation;
const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
1e-5), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
};
/**
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
*/
template<class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
D> {
template<class CAMERA>
class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
public:
private:
typedef SmartFactorBase<CAMERA> Base;
typedef SmartProjectionFactor<CAMERA> This;
typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
protected:
// Some triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
/// @name Parameters
/// @{
const SmartProjectionParams params_;
/// @}
/// @name Caching triangulation
/// @{
mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_;
mutable bool cheiralityException_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
boost::shared_ptr<SmartProjectionFactorState> state_;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<PinholeCamera<CALIBRATION>, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<CALIBRATION, D> This;
/// @}
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a pinhole camera
typedef PinholeCamera<CALIBRATION> Camera;
typedef CameraSet<Camera> Cameras;
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from sensor to body frame (default identity)
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<Pose3> body_P_sensor = boost::none,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
SmartFactorStatePtr(new SmartProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold) {
SmartProjectionFactor(
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) {
}
/** Virtual destructor */
@ -147,24 +173,33 @@ public:
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionFactor, z = \n";
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode
<< std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation
<< std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
/// Check if the new linearization point_ is the same as the one used for previous triangulation
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol);
}
/// Check if the new linearization point is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
size_t m = cameras.size();
bool retriangulate = false;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
// if we do not have a previous linearization point or the new linearization point includes more poses
if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true;
@ -172,7 +207,7 @@ public:
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) {
params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
@ -187,137 +222,34 @@ public:
cameraPosesTriangulation_.push_back(cameras[i].pose());
}
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// This function checks if the new linearization point_ is 'close' to the previous one used for linearization
bool decideIfLinearize(const Cameras& cameras) const {
// "selective linearization"
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
// (we only care about the "rigidity" of the poses, not about their absolute pose)
if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
return true;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
if (cameraPosesLinearization_.empty()
|| (cameras.size() != cameraPosesLinearization_.size()))
return true;
Pose3 firstCameraPose, firstCameraPoseOld;
for (size_t i = 0; i < cameras.size(); i++) {
if (i == 0) { // we store the initial pose, this is useful for selective re-linearization
firstCameraPose = cameras[i].pose();
firstCameraPoseOld = cameraPosesLinearization_[i];
continue;
}
// we compare the poses in the frame of the first pose
Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
Pose3 localCameraPoseOld = firstCameraPoseOld.between(
cameraPosesLinearization_[i]);
if (!localCameraPose.equals(localCameraPoseOld,
this->linearizationThreshold_))
return true; // at least two "relative" poses are different, hence we re-linearize
}
return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
}
/// triangulateSafe
size_t triangulateSafe(const Cameras& cameras) const {
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true;
return m;
}
if (m < 2) // if we have a single pose the corresponding factor is uninformative
return TriangulationResult::Degenerate();
bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
point_ = triangulatePoint3<Camera>(cameras, this->measured_,
rankTolerance_, enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
degenerate_ = true;
cheiralityException_ = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
}
}
return m;
if (retriangulate)
result_ = gtsam::triangulateSafe(cameras, this->measured_,
params_.triangulation);
return result_;
}
/// triangulate
bool triangulateForLinearize(const Cameras& cameras) const {
bool isDebug = false;
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout
<< "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
} else {
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
return true;
}
triangulateSafe(cameras); // imperative, might reset result_
return (result_);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0) const {
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
false) const {
bool isDebug = false;
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
std::vector<Key> js;
@ -331,264 +263,198 @@ public:
exit(1);
}
this->triangulateSafe(cameras);
triangulateSafe(cameras);
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
// std::cout << "In linearize: exception" << std::endl;
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
0.0);
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
bool doLinearize = this->decideIfLinearize(cameras);
if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
if (!doLinearize) { // return the previous Hessian factor
std::cout << "=============================" << std::endl;
std::cout << "doLinearize " << doLinearize << std::endl;
std::cout << "this->linearizationThreshold_ "
<< this->linearizationThreshold_ << std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
std::cout
<< "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
<< std::endl;
exit(1);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
this->state_->Gs, this->state_->gs, this->state_->f);
}
// ==================================================================
Matrix F, E;
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<typename Base::MatrixZD> Fblocks;
Matrix E;
Vector b;
double f = computeJacobians(F, E, b, cameras);
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
// And we care, as in grouped factors this is called repeatedly
Matrix H(D * numKeys, D * numKeys);
Vector gs_vector;
// Whiten using noise model
Base::whitenJacobians(Fblocks, E, b);
Matrix3 P = Base::PointCov(E, lambda);
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment<D>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
GsCount2++;
}
}
}
// ==================================================================
if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
this->state_->Gs = Gs;
this->state_->gs = gs;
this->state_->f = f;
}
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
augmentedHessian);
}
// create factor
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
else
return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
// failed: return empty
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
}
/// create factor
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
return Base::createJacobianQFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
// failed: return empty
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
}
/// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const {
Cameras myCameras;
// TODO triangulate twice ??
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
else
return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
return createJacobianQFactor(this->cameras(values), lambda);
}
/// different (faster) way to compute Jacobian factor
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorSVD<D, 2> >(this->keys_);
// failed: return empty
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& myCameras) const {
Values valuesFactor;
/// linearize to a Hessianfactor
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
const Values& values, double lambda = 0.0) const {
return createHessianFactor(this->cameras(values), lambda);
}
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
/// linearize to an Implicit Schur factor
virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
const Values& values, double lambda = 0.0) const {
return createRegularImplicitSchurFactor(this->cameras(values), lambda);
}
myCameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(myCameras);
/// linearize to a JacobianfactorQ
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
const Values& values, double lambda = 0.0) const {
return createJacobianQFactor(this->cameras(values), lambda);
}
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_)))
return false;
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartProjectionFactor: this is not ready" << std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
case IMPLICIT_SCHUR:
return createRegularImplicitSchurFactor(cameras, lambda);
case JACOBIAN_SVD:
return createJacobianSVDFactor(cameras, lambda);
case JACOBIAN_Q:
return createJacobianQFactor(cameras, lambda);
default:
throw std::runtime_error("SmartFactorlinearize: unknown mode");
}
return true;
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
return Base::computeEP(E, P, cameras, point_);
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
Cameras cameras = this->cameras(values);
return linearizeDamped(cameras, lambda);
}
/// Takes values
bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
/// linearize
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return linearizeDamped(values);
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeEP(E, P, myCameras);
cameras.project2(*result_, boost::none, E);
return nonDegenerate;
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, myCameras);
return nonDegenerate;
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateAndComputeE(E, cameras);
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
void computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (this->degenerate_) {
std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
std::cout << "point " << point_ << std::endl;
std::cout
<< "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used"
<< std::endl;
if (D > 6) {
std::cout
<< "Management of degeneracy is not yet ready when one also optimizes for the calibration "
<< std::endl;
}
int numKeys = this->keys_.size();
E = zeros(2 * numKeys, 2);
b = zero(2 * numKeys);
double f = 0;
for (size_t i = 0; i < this->measured_.size(); i++) {
if (i == 0) { // first pose
this->point_ = cameras[i].backprojectPointAtInfinity(
this->measured_.at(i));
// 3D parametrization of point at infinity: [px py 1]
}
Matrix Fi, Ei;
Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
- this->measured_.at(i)).vector();
this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
f += bi.squaredNorm();
Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
E.block<2, 2>(2 * i, 0) = Ei;
subInsert(b, bi, 2 * i);
}
return f;
if (!result_) {
// Handle degeneracy
// TODO check flag whether we should do this
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
this->measured_.at(0));
Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
} else {
// nondegenerate: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else
// valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, *result_);
}
}
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Values& values) const {
typename Base::Cameras myCameras;
double good = computeCamerasAndTriangulate(values, myCameras);
if (good)
computeJacobiansSVD(Fblocks, Enull, b, myCameras);
return true;
}
/// SVD version
double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& Enull, Vector& b, const Cameras& cameras) const {
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
}
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
// TODO should there be a lambda?
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
const Cameras& cameras) const {
return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
}
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras) const {
return Base::computeJacobians(F, E, b, cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
/// Assumes triangulation was done and degeneracy handled
Vector reprojectionError(const Cameras& cameras) const {
return Base::reprojectionError(cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return reprojectionError(myCameras);
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return zero(myCameras.size() * 2);
return zero(cameras.size() * 2);
}
/**
@ -600,86 +466,57 @@ public:
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras;
if (externalPoint) {
nrCameras = this->keys_.size();
point_ = *externalPoint;
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
return Base::totalReprojectionError(cameras, backprojected);
} else
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
}
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
this->degenerate_ = true;
}
if (this->degenerate_) {
// return 0.0; // TODO: this maybe should be zero?
std::cout
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
<< std::endl;
size_t i = 0;
double overallError = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
const Point2& zi = this->measured_.at(i);
if (i == 0) // first pose
this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
Point2 reprojectionError(
camera.projectPointAtInfinity(this->point_) - zi);
overallError += 0.5
* this->noise_.at(i)->distance(reprojectionError.vector());
i += 1;
}
return overallError;
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);
/// Calculate total reprojection error
virtual double error(const Values& values) const {
if (this->active(values)) {
return totalReprojectionError(Base::cameras(values));
} else { // else of active flag
return 0.0;
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
TriangulationResult point() const {
return result_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
/// Is result valid?
bool isValid() const {
return result_;
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
bool isDegenerate() const {
return result_.degenerate();
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return cheirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const {
return throwCheirality_;
bool isPointBehindCamera() const {
return result_.behindCamera();
}
private:
@ -687,11 +524,18 @@ private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
}
}
;
/// traits
template<class CAMERA>
struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
SmartProjectionFactor<CAMERA> > {
};
} // \ namespace gtsam

160
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -38,87 +38,37 @@ namespace gtsam {
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> {
class SmartProjectionPoseFactor: public SmartProjectionFactor<
PinholePose<CALIBRATION> > {
private:
typedef PinholePose<CALIBRATION> Camera;
typedef SmartProjectionFactor<Camera> Base;
typedef SmartProjectionPoseFactor<CALIBRATION> This;
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
boost::shared_ptr<CALIBRATION> K_; ///< calibration object (one for all cameras)
public:
/// shorthand for base class type
typedef SmartProjectionFactor<CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartProjectionPoseFactor<CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from sensor to body frame (default identity)
* @param K (fixed) calibration, assumed to be the same for all cameras
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor, params), K_(K) {
}
/** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {}
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
*/
void add(const Point2 measured_i, const Key poseKey_i,
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
K_all_.push_back(K_i);
}
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
Base::add(measurements, poseKeys, noises);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
}
}
/**
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
K_all_.push_back(K);
}
virtual ~SmartProjectionPoseFactor() {
}
/**
@ -129,59 +79,15 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionPoseFactor, z = \n ";
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol);
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if(Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
typename Base::Camera camera(pose, *K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
// depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){
case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0);
break;
case JACOBIAN_Q :
return this->createJacobianQFactor(cameras(values), 0.0);
break;
default:
return this->createHessianFactor(cameras(values));
break;
}
}
/**
* error calculates the error of the factor.
*/
@ -193,9 +99,28 @@ public:
}
}
/** return the calibration object */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
return K_all_;
/** return calibration shared pointers */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
Camera camera(pose, K_);
cameras.push_back(camera);
}
return cameras;
}
private:
@ -205,10 +130,11 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
}; // end of class declaration
};
// end of class declaration
/// traits
template<class CALIBRATION>

29
gtsam/slam/TriangulationFactor.h
View File

@ -16,8 +16,7 @@
*/
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/optional.hpp>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/make_shared.hpp>
namespace gtsam {
@ -27,18 +26,24 @@ namespace gtsam {
* The calibration and pose are assumed known.
* @addtogroup SLAM
*/
template<class CALIBRATION = Cal3_S2>
template<class CAMERA>
class TriangulationFactor: public NoiseModelFactor1<Point3> {
public:
/// Camera type
typedef PinholeCamera<CALIBRATION> Camera;
/// CAMERA type
typedef CAMERA Camera;
protected:
/// shorthand for base class type
typedef NoiseModelFactor1<Point3> Base;
/// shorthand for this class
typedef TriangulationFactor<CAMERA> This;
// Keep a copy of measurement and calibration for I/O
const Camera camera_; ///< Camera in which this landmark was seen
const CAMERA camera_; ///< CAMERA in which this landmark was seen
const Point2 measured_; ///< 2D measurement
// verbosity handling for Cheirality Exceptions
@ -47,12 +52,6 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor1<Point3> Base;
/// shorthand for this class
typedef TriangulationFactor<CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -70,7 +69,7 @@ public:
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
TriangulationFactor(const Camera& camera, const Point2& measured,
TriangulationFactor(const CAMERA& camera, const Point2& measured,
const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
@ -114,7 +113,7 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
Point2 error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
} catch (CheiralityException& e) {
if (H2)
@ -154,7 +153,7 @@ public:
// Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key());
b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector();
b = -(camera_.project2(point, boost::none, A) - measured_).vector();
if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b);

50
gtsam/slam/expressions.h
View File

@ -28,6 +28,7 @@ inline Point2_ transform_to(const Pose2_& x, const Point2_& p) {
// 3D Geometry
typedef Expression<Point3> Point3_;
typedef Expression<Unit3> Unit3_;
typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_;
@ -40,33 +41,52 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
typedef Expression<Cal3_S2> Cal3_S2_;
typedef Expression<Cal3Bundler> Cal3Bundler_;
/// Expression version of PinholeBase::Project
inline Point2_ project(const Point3_& p_cam) {
return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
return Point2_(f, p_cam);
}
template <class CAMERA>
Point2 project4(const CAMERA& camera, const Point3& p,
OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) {
inline Point2_ project(const Unit3_& p_cam) {
Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
return Point2_(f, p_cam);
}
namespace internal {
// Helper template for project2 expression below
template<class CAMERA, class POINT>
Point2 project4(const CAMERA& camera, const POINT& p,
OptionalJacobian<2, CAMERA::dimension> Dcam,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
return camera.project2(p, Dcam, Dpoint);
}
template <class CAMERA>
Point2_ project2(const Expression<CAMERA>& camera_, const Point3_& p_) {
return Point2_(project4<CAMERA>, camera_, p_);
}
template<class CAMERA, class POINT>
Point2_ project2(const Expression<CAMERA>& camera_,
const Expression<POINT>& p_) {
return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
}
namespace internal {
// Helper template for project3 expression below
template<class CALIBRATION, class POINT>
inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
OptionalJacobian<2, 5> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}
inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
return Point2_(project6, x, p, K);
}
template<class CAL>
Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
return Point2_(K, &CAL::uncalibrate, xy_hat);
template<class CALIBRATION, class POINT>
inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
const Expression<CALIBRATION>& K) {
return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
}
template<class CALIBRATION>
Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
}
} // \namespace gtsam

145
gtsam/slam/tests/smartFactorScenarios.h Normal file
View File

@ -0,0 +1,145 @@
/* ----------------------------------------------------------------------------
* 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 SmartFactorScenarios.h
* @brief Scenarios for testSmart*.cpp
* @author Frank Dellaert
* @date Feb 2015
*/
#pragma once
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
using namespace std;
using namespace gtsam;
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
Point3 landmark4(10, 0.5, 1.2);
Point3 landmark5(10, -0.5, 1.2);
// First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
// Second camera 1 meter to the right of first camera
Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// Third camera 1 meter above the first camera
Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
// Create a noise unit2 for the pixel error
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static double fov = 60; // degrees
static size_t w = 640, h = 480;
/* ************************************************************************* */
// default Cal3_S2 cameras
namespace vanilla {
typedef PinholeCamera<Cal3_S2> Camera;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3_S2 K(fov, w, h);
static Cal3_S2 K2(1500, 1200, 0, w, h);
Camera level_camera(level_pose, K2);
Camera level_camera_right(pose_right, K2);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Camera cam1(level_pose, K2);
Camera cam2(pose_right, K2);
Camera cam3(pose_above, K2);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
SmartProjectionParams params;
}
/* ************************************************************************* */
// default Cal3_S2 poses
namespace vanillaPose {
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK);
Camera cam1(level_pose, sharedK);
Camera cam2(pose_right, sharedK);
Camera cam3(pose_above, sharedK);
}
/* ************************************************************************* */
// default Cal3_S2 poses
namespace vanillaPose2 {
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
Camera level_camera(level_pose, sharedK2);
Camera level_camera_right(pose_right, sharedK2);
Camera cam1(level_pose, sharedK2);
Camera cam2(pose_right, sharedK2);
Camera cam3(pose_above, sharedK2);
}
/* *************************************************************************/
// Cal3Bundler cameras
namespace bundler {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
Camera level_camera(level_pose, K);
Camera level_camera_right(pose_right, K);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Pose3 pose1 = level_pose;
Camera cam1(level_pose, K);
Camera cam2(pose_right, K);
Camera cam3(pose_above, K);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
}
/* *************************************************************************/
// Cal3Bundler poses
namespace bundlerPose {
typedef PinholePose<Cal3Bundler> Camera;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
Camera level_camera(level_pose, sharedBundlerK);
Camera level_camera_right(pose_right, sharedBundlerK);
Camera cam1(level_pose, sharedBundlerK);
Camera cam2(pose_right, sharedBundlerK);
Camera cam3(pose_above, sharedBundlerK);
}
/* *************************************************************************/
template<class CAMERA>
CAMERA perturbCameraPose(const CAMERA& camera) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
return CAMERA(perturbedCameraPose, camera.calibration());
}
template<class CAMERA>
void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
const CAMERA& cam3, Point3 landmark, vector<Point2>& measurements_cam) {
Point2 cam1_uv1 = cam1.project(landmark);
Point2 cam2_uv1 = cam2.project(landmark);
Point2 cam3_uv1 = cam3.project(landmark);
measurements_cam.push_back(cam1_uv1);
measurements_cam.push_back(cam2_uv1);
measurements_cam.push_back(cam3_uv1);
}
/* ************************************************************************* */

2
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) {
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = SimpleCamera::project_to_camera(P2);
const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();

90
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -19,11 +19,13 @@
#include <gtsam/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQR.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/timing.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/base/timing.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
@ -39,8 +41,8 @@ using namespace gtsam;
const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3);
const FastVector<Key> keys = list_of<Key>(0)(1)(3);
// RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
@ -51,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) {
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P));
}
@ -84,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys;
keys += 0,1,2,3;
Vector x = xvalues.vector(keys);
FastVector<Key> keys2;
keys2 += 0,1,2,3;
Vector x = xvalues.vector(keys2);
Vector expected = zero(24);
RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
// Create ImplicitSchurFactor
RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
RegularImplicitSchurFactor<CalibratedCamera> implicitFactor(keys, FBlocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version
@ -122,32 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactor with same error
const SharedDiagonal model;
JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model);
{ // error
double expectedError = jf.error(xvalues);
double actualError = implicitFactor.errorJF(xvalues);
DOUBLES_EQUAL(expectedError,actualError,1e-7)
// error
double expectedError = 11875.083333333334;
{
EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7)
EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7)
EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7)
}
{ // JacobianFactor with same error
{
VectorValues yActual = zero;
jf.multiplyHessianAdd(alpha, xvalues, yActual);
jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(alpha, xvalues, yActual);
jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(-1, xvalues, yActual);
jfQ.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8));
}
{ // check hessian Diagonal
VectorValues diagExpected = jf.hessianDiagonal();
VectorValues diagExpected = jfQ.hessianDiagonal();
VectorValues diagActual = implicitFactor.hessianDiagonal();
EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
}
{ // check hessian Block Diagonal
map<Key,Matrix> BD = jf.hessianBlockDiagonal();
map<Key,Matrix> BD = jfQ.hessianBlockDiagonal();
map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(BD[0],actualBD[0]));
@ -157,40 +161,46 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
{ // Raw memory Version
std::fill(y, y + 24, 0);// zero y !
jf.multiplyHessianAdd(alpha, xdata, y);
jfQ.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8));
jf.multiplyHessianAdd(alpha, xdata, y);
jfQ.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jf.multiplyHessianAdd(-1, xdata, y);
jfQ.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
}
VectorValues expectedVV;
expectedVV.insert(0,-3.5*ones(6));
expectedVV.insert(1,10*ones(6)/3);
expectedVV.insert(3,-19.5*ones(6));
{ // Check gradientAtZero
VectorValues expected = jf.gradientAtZero();
VectorValues actual = implicitFactor.gradientAtZero();
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8));
EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8));
}
// Create JacobianFactorQR
JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model);
EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7)
EXPECT(assert_equal(expectedVV, jfQR.gradientAtZero(), 1e-8));
{
const SharedDiagonal model;
VectorValues yActual = zero;
jfq.multiplyHessianAdd(alpha, xvalues, yActual);
jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(alpha, xvalues, yActual);
jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(-1, xvalues, yActual);
jfQR.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8));
}
{ // Raw memory Version
std::fill(y, y + 24, 0);// zero y !
jfq.multiplyHessianAdd(alpha, xdata, y);
jfQR.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8));
jfq.multiplyHessianAdd(alpha, xdata, y);
jfQR.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jfq.multiplyHessianAdd(-1, xdata, y);
jfQR.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
}
delete [] y;
@ -214,7 +224,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
RegularImplicitSchurFactor<CalibratedCamera> factor(keys, FBlocks, E, P, b);
// hessianDiagonal
VectorValues expected;
@ -255,6 +265,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// augmentedInformation (test just checks diagonals)
Matrix actualInfo = factor.augmentedInformation();
EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
// information (test just checks diagonals)
Matrix actualInfo2 = factor.information();
EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
}
/* ************************************************************************* */

4
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler>, 9> {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
virtual double error(const Values& values) const {
return 0.0;
}
@ -45,7 +45,7 @@ TEST(SmartFactorBase, Pinhole) {
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
class StereoFactor: public SmartFactorBase<StereoCamera, 9> {
class StereoFactor: public SmartFactorBase<StereoCamera> {
virtual double error(const Values& values) const {
return 0.0;
}

856
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp Normal file
View File

@ -0,0 +1,856 @@
/* ----------------------------------------------------------------------------
* 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 testSmartProjectionCameraFactor.cpp
* @brief Unit tests for SmartProjectionCameraFactor Class
* @author Chris Beall
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @date Sept 2013
*/
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
using namespace boost::assign;
static bool isDebugTest = false;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
static Key x1(1);
Symbol l1('l', 1), l2('l', 2), l3('l', 3);
Key c1 = 1, c2 = 2, c3 = 3;
static Point2 measurement1(323.0, 240.0);
static double rankTol = 1.0;
template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
const PinholeCamera<CALIBRATION>& camera) {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
typename gtsam::traits<CALIBRATION>::TangentVector d;
d.setRandom();
d *= 0.1;
CALIBRATION perturbedCalibration = camera.calibration().retract(d);
return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, perturbCameraPose) {
using namespace vanilla;
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
Camera actualCamera(perturbed_level_pose, K2);
Camera expectedCamera = perturbCameraPose(level_camera);
CHECK(assert_equal(expectedCamera, actualCamera));
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor3) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
factor1.add(measurement1, x1, unit2);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Equals ) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, x1, unit2);
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiseless ) {
using namespace vanilla;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
CHECK(
assert_equal(zero(4),
factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noisy ) {
using namespace vanilla;
// Project one landmark into two cameras and add noise on first
Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
Point2 level_uv_right = level_camera_right.project(landmark1);
Values values;
values.insert(c1, level_camera);
Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
values.insert(c2, perturbed_level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
// Point is now uninitialized before a triangulation event
EXPECT(!factor1->point());
double expectedError = 58640;
double actualError1 = factor1->error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);
// Check triangulated point
CHECK(factor1->point());
EXPECT(
assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));
// Check whitened errors
Vector expected(4);
expected << -7, 235, 58, -242;
SmartFactor::Cameras cameras1 = factor1->cameras(values);
Point3 point1 = *factor1->point();
Vector actual = factor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor());
vector<Point2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(unit2);
noises.push_back(unit2);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
factor2->add(measurements, views, noises);
double actualError2 = factor2->error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create and fill smartfactors
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
smartFactor1->add(measurements_cam1, views, unit2);
smartFactor2->add(measurements_cam2, views, unit2);
smartFactor3->add(measurements_cam3, views, unit2);
// Create factor graph and add priors on two cameras
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
// Create initial estimate
Values initial;
initial.insert(c1, cam1);
initial.insert(c2, cam2);
initial.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
// Points are now uninitialized before a triangulation event
EXPECT(!smartFactor1->point());
EXPECT(!smartFactor2->point());
EXPECT(!smartFactor3->point());
EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
// Error should trigger this and initialize the points, abort if not so
CHECK(smartFactor1->point());
CHECK(smartFactor2->point());
CHECK(smartFactor3->point());
EXPECT(
assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
1e-4));
EXPECT(
assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
1e-4));
EXPECT(
assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
1e-4));
// Check whitened errors
Vector expected(6);
expected << 256, 29, -26, 29, -206, -202;
Point3 point1 = *smartFactor1->point();
SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
Vector reprojectionError = cameras1.reprojectionError(point1,
measurements_cam1);
EXPECT(assert_equal(expected, reprojectionError, 1));
Vector actual = smartFactor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
// Optimize
LevenbergMarquardtParams lmParams;
if (isDebugTest) {
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
}
LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
Values result = optimizer.optimize();
EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7));
EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7));
EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7));
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla;
// Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SfM_Track track1;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(track1, unit2);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(track2, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
values.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
using namespace vanilla;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(smartFactor5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
values.insert(c3, perturbCameraPoseAndCalibration(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
lmParams.relativeErrorTol = 1e-8;
lmParams.absoluteErrorTol = 0;
lmParams.maxIterations = 20;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 20));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(c3, perturbCameraPose(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
lmParams.relativeErrorTol = 1e-8;
lmParams.absoluteErrorTol = 0;
lmParams.maxIterations = 20;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 1));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
Values values;
values.insert(c1, cam1);
values.insert(c2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(c3, perturbCameraPoseAndCalibration(cam3));
if (isDebugTest)
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams lmParams;
lmParams.relativeErrorTol = 1e-8;
lmParams.absoluteErrorTol = 0;
lmParams.maxIterations = 20;
if (isDebugTest)
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
if (isDebugTest)
result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
EXPECT(
assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiselessBundler ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
double actualError = factor1->error(values);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-3);
Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this)
if (factor1->point())
oldPoint = *(factor1->point());
Point3 expectedPoint;
if (factor1->point(values))
expectedPoint = *(factor1->point(values));
EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
smartGraph.push_back(factor1);
double expectedError = factor1->error(values);
double expectedErrorGraph = smartGraph.error(values);
Point3 expectedPoint;
if (factor1->point())
expectedPoint = *(factor1->point());
// cout << "expectedPoint " << expectedPoint.vector() << endl;
// COMMENTS:
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
// value in the generalGrap
NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1);
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
generalGraph.push_back(sfm1);
generalGraph.push_back(sfm2);
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
double actualError = 0.5
* (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
double actualErrorGraph = generalGraph.error(values);
DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
smartGraph.push_back(factor1);
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
Point3 expectedPoint;
if (factor1->point())
expectedPoint = *(factor1->point());
// COMMENTS:
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
// value in the generalGrap
NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1);
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
generalGraph.push_back(sfm1);
generalGraph.push_back(sfm2);
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
- actualFullHessian.block(0, 18, 18, 3)
* (actualFullHessian.block(18, 18, 3, 3)).inverse()
* actualFullHessian.block(18, 0, 3, 18);
Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
- actualFullHessian.block(0, 18, 18, 3)
* (actualFullHessian.block(18, 18, 3, 3)).inverse()
* actualFullInfoVector.block(18, 0, 3, 1);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
}
/* *************************************************************************/
// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
//
// Values values;
// values.insert(c1, level_camera);
// values.insert(c2, level_camera_right);
//
// NonlinearFactorGraph smartGraph;
// SmartFactor::shared_ptr factor1(new SmartFactor());
// factor1->add(level_uv, c1, unit2);
// factor1->add(level_uv_right, c2, unit2);
// smartGraph.push_back(factor1);
// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
//
// Point3 expectedPoint;
// if(factor1->point())
// expectedPoint = *(factor1->point());
//
// // COMMENTS:
// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
// // value in the generalGrap
// NonlinearFactorGraph generalGraph;
// SFMFactor sfm1(level_uv, unit2, c1, l1);
// SFMFactor sfm2(level_uv_right, unit2, c2, l1);
// generalGraph.push_back(sfm1);
// generalGraph.push_back(sfm2);
// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
// GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values);
//
// double alpha = 1.0;
//
// VectorValues yExpected, yActual, ytmp;
// VectorValues xtmp = map_list_of
// (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0))
// (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0))
// (l1, (Vec(3) << 5.5, 0.5, 1.2));
// gfg ->multiplyHessianAdd(alpha, xtmp, ytmp);
//
// VectorValues x = map_list_of
// (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9))
// (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19))
// (l1, (Vec(3) << 5.5, 0.5, 1.2));
//
// gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual);
// gfg ->multiplyHessianAdd(alpha, x, yExpected);
//
// EXPECT(assert_equal(yActual,yExpected, 1e-7));
//}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
Matrix expectedE;
Vector expectedb;
CameraSet<Camera> cameras;
cameras.push_back(level_camera);
cameras.push_back(level_camera_right);
factor1->error(values); // this is important to have a triangulation of the point
Point3 point;
if (factor1->point())
point = *(factor1->point());
vector<Matrix29> Fblocks;
factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1);
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
generalGraph.push_back(sfm1);
generalGraph.push_back(sfm2);
values.insert(l1, point); // note: we get rid of possible errors in the triangulation
Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();
Matrix3 expectedVinv = factor1->PointCov(expectedE);
EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7));
}
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
using namespace bundler;
Values values;
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
double rankTol = 1;
bool useEPI = false;
// Hessian version
SmartProjectionParams params;
params.setRankTolerance(rankTol);
params.setLinearizationMode(gtsam::HESSIAN);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(useEPI);
SmartFactor::shared_ptr explicitFactor(
new SmartFactor(boost::none, params));
explicitFactor->add(level_uv, c1, unit2);
explicitFactor->add(level_uv_right, c2, unit2);
GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
values);
HessianFactor& hessianFactor =
dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
// Implicit Schur version
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
SmartFactor::shared_ptr implicitFactor(
new SmartFactor(boost::none, params));
implicitFactor->add(level_uv, c1, unit2);
implicitFactor->add(level_uv_right, c2, unit2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
implicitFactor->linearize(values);
CHECK(gaussianImplicitSchurFactor);
typedef RegularImplicitSchurFactor<Camera> Implicit9;
Implicit9& implicitSchurFactor =
dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);
VectorValues x = map_list_of(c1,
(Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2,
(Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished());
VectorValues yExpected, yActual;
double alpha = 1.0;
hessianFactor.multiplyHessianAdd(alpha, x, yActual);
implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected);
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

1447
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

File diff suppressed because it is too large Load Diff

5
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -35,7 +36,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
SimpleCamera camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
@ -48,7 +49,7 @@ TEST( triangulation, TriangulationFactor ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<> Factor;
typedef TriangulationFactor<SimpleCamera> Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians

32
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -26,16 +26,14 @@
* -makes monocular observations of many landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <string>
#include <fstream>
@ -46,6 +44,7 @@ using namespace gtsam;
int main(int argc, char** argv){
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
Values initial_estimate;
@ -68,18 +67,17 @@ int main(int argc, char** argv){
cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str());
int pose_id;
int i;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
while (pose_file >> i) {
for (int i = 0; i < 16; i++)
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
initial_estimate.insert(i, Pose3(m));
}
// camera and landmark keys
size_t x, l;
// landmark keys
size_t l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
@ -89,24 +87,24 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor());
SmartFactor::shared_ptr factor(new SmartFactor(K));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor());
factor = SmartFactor::shared_ptr(new SmartFactor(K));
current_l = l;
}
factor->add(Point2(uL,v), Symbol('x',x), model, K);
factor->add(Point2(uL,v), i, model);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
Pose3 firstPose = initial_estimate.at<Pose3>(1);
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.push_back(NonlinearEquality<Pose3>(1,firstPose));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

356
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -33,49 +33,25 @@
namespace gtsam {
/**
* Structure for storing some state memory, used to speed up optimization
* @addtogroup SLAM
*/
class SmartStereoProjectionFactorState {
protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SmartStereoProjectionFactorState() {
}
// Hessian representation (after Schur complement)
bool calculatedHessian;
Matrix H;
Vector gs_vector;
std::vector<Matrix> Gs;
std::vector<Vector> gs;
double f;
};
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
/**
* SmartStereoProjectionFactor: triangulates point
*/
template<class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> {
template<class CALIBRATION>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
protected:
// Some triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
/// @name Caching triangulation
/// @{
const TriangulationParameters parameters_;
// TODO:
// mutable TriangulationResult result_; ///< result from triangulateSafe
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
/// @}
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
@ -84,29 +60,22 @@ protected:
mutable bool degenerate_;
mutable bool cheiralityException_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
boost::shared_ptr<SmartStereoProjectionFactorState> state_;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<StereoCamera, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
typedef SmartFactorBase<StereoCamera> Base;
/// shorthand for this class
typedef SmartStereoProjectionFactor<CALIBRATION, D> This;
typedef SmartStereoProjectionFactor<CALIBRATION> This;
enum {ZDim = 3}; ///< Dimension trait of measurement type
enum {
ZDim = 3
}; ///< Dimension trait of measurement type
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
public:
@ -128,18 +97,15 @@ public:
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<Pose3> body_P_sensor = boost::none,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
SmartStereoProjectionFactor(const double rankTolerance,
const double linThreshold, const bool manageDegeneracy,
const bool enableEPI, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state),
landmarkDistanceThreshold_(landmarkDistanceThreshold),
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
false), verboseCheirality_(false) {
}
/** Virtual destructor */
@ -153,11 +119,12 @@ public:
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionFactor, z = \n";
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
std::cout << s << "SmartStereoProjectionFactor\n";
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
<< std::endl;
Base::print("", keyFormatter);
}
@ -197,40 +164,6 @@ public:
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// This function checks if the new linearization point_ is 'close' to the previous one used for linearization
bool decideIfLinearize(const Cameras& cameras) const {
// "selective linearization"
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
// (we only care about the "rigidity" of the poses, not about their absolute pose)
if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
return true;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
if (cameraPosesLinearization_.empty()
|| (cameras.size() != cameraPosesLinearization_.size()))
return true;
Pose3 firstCameraPose, firstCameraPoseOld;
for (size_t i = 0; i < cameras.size(); i++) {
if (i == 0) { // we store the initial pose, this is useful for selective re-linearization
firstCameraPose = cameras[i].pose();
firstCameraPoseOld = cameraPosesLinearization_[i];
continue;
}
// we compare the poses in the frame of the first pose
Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
Pose3 localCameraPoseOld = firstCameraPoseOld.between(
cameraPosesLinearization_[i]);
if (!localCameraPose.equals(localCameraPoseOld,
this->linearizationThreshold_))
return true; // at least two "relative" poses are different, hence we re-linearize
}
return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
@ -268,7 +201,7 @@ public:
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
}
point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
rankTolerance_, enableEPI_);
parameters_.rankTolerance, parameters_.enableEPI);
// // // End temporary hack
@ -281,11 +214,11 @@ public:
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i=0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
degenerate_ = true;
break;
}
@ -300,8 +233,8 @@ public:
}
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// we discard smart factors that have large reprojection error
if(dynamicOutlierRejectionThreshold_ > 0 &&
totalReprojError/m > dynamicOutlierRejectionThreshold_)
if (parameters_.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
@ -345,15 +278,15 @@ public:
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0) const {
bool isDebug = false;
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
std::vector < Key > js;
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
std::vector < Vector > gs(numKeys);
std::vector<Key> js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size()) {
std::cout
@ -362,138 +295,123 @@ public:
exit(1);
}
this->triangulateSafe(cameras);
if (isDebug) std::cout << "point_ = " << point_ << std::endl;
triangulateSafe(cameras);
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) std::cout << "In linearize: exception" << std::endl;
if (isDebug)
std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
0.0);
}
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (isDebug) std::cout << "degenerate_ = true" << std::endl;
if (isDebug)
std::cout << "degenerate_ = true" << std::endl;
}
bool doLinearize = this->decideIfLinearize(cameras);
if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl;
if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
if (!doLinearize) { // return the previous Hessian factor
std::cout << "=============================" << std::endl;
std::cout << "doLinearize " << doLinearize << std::endl;
std::cout << "this->linearizationThreshold_ "
<< this->linearizationThreshold_ << std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
std::cout
<< "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
<< std::endl;
exit(1);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
this->state_->Gs, this->state_->gs, this->state_->f);
}
// ==================================================================
std::vector<typename Base::MatrixZD> Fblocks;
Matrix F, E;
Vector b;
double f = computeJacobians(F, E, b, cameras);
computeJacobians(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks, F); // expensive !!!
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
// And we care, as in grouped factors this is called repeatedly
Matrix H(D * numKeys, D * numKeys);
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
Vector gs_vector;
Matrix3 P = Base::PointCov(E,lambda);
Matrix3 P = Cameras::PointCov(E, lambda);
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug) std::cout << "H:\n" << H << std::endl;
if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug)
std::cout << "H:\n" << H << std::endl;
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment < D > (i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * Base::Dim;
gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
GsCount2++;
}
}
}
// ==================================================================
if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
this->state_->Gs = Gs;
this->state_->gs = gs;
this->state_->f = f;
}
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f);
double f = b.squaredNorm();
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
}
// // create factor
// boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
// boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createImplicitSchurFactor(cameras, point_, lambda);
// else
// return boost::shared_ptr<ImplicitSchurFactor<D> >();
// return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
// }
//
// /// create factor
// boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createJacobianQFactor(cameras, point_, lambda);
// else
// return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// }
//
// /// Create a factor, takes values
// boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// const Values& values, double lambda) const {
// Cameras myCameras;
// Cameras cameras;
// // TODO triangulate twice ??
// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
// if (nonDegenerate)
// return createJacobianQFactor(myCameras, lambda);
// return createJacobianQFactor(cameras, lambda);
// else
// return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// }
//
/// different (faster) way to compute Jacobian factor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
double lambda) const {
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& myCameras) const {
Cameras& cameras) const {
Values valuesFactor;
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
myCameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(myCameras);
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
@ -505,7 +423,8 @@ public:
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl;
std::cout << "SmartStereoProjectionFactor: this is not ready"
<< std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
@ -513,34 +432,32 @@ public:
return true;
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
Base::computeEP(E, PointCov, cameras, point_);
}
/// Takes values
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeEP(E, PointCov, myCameras);
cameras.project2(point_, boost::none, E);
return nonDegenerate;
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, myCameras, 0.0);
computeJacobians(Fblocks, E, b, cameras, 0.0);
return nonDegenerate;
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!");
@ -571,73 +488,36 @@ public:
//
// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
// f += bi.squaredNorm();
// Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
// E.block < 2, 2 > (2 * i, 0) = Ei;
// subInsert(b, bi, 2 * i);
// }
// return f;
} else {
// nondegenerate: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, point_);
Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else
}
// /// Version that computes PointCov, with optional lambda parameter
// double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
// const double lambda = 0.0) const {
//
// double f = computeJacobians(Fblocks, E, b, cameras);
//
// // Point covariance inv(E'*E)
// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
//
// return f;
// }
//
// /// takes values
// bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& Enull, Vector& b, const Values& values) const {
// typename Base::Cameras myCameras;
// double good = computeCamerasAndTriangulate(values, myCameras);
// if (good)
// computeJacobiansSVD(Fblocks, Enull, b, myCameras);
// return true;
// }
//
// /// SVD version
// double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
// Matrix& Enull, Vector& b, const Cameras& cameras) const {
// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
// }
//
// /// Returns Matrix, TODO: maybe should not exist -> not sparse !
// // TODO should there be a lambda?
// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
// const Cameras& cameras) const {
// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
// }
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
double computeJacobians(Matrix& F, Matrix& E, Vector& b,
const Cameras& cameras) const {
return Base::computeJacobians(F, E, b, cameras, point_);
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
}
/// Calculate vector of re-projection errors, before applying noise model
/// Assumes triangulation was done and degeneracy handled
Vector reprojectionError(const Cameras& cameras) const {
return Base::reprojectionError(cameras, point_);
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return reprojectionError(myCameras);
return Base::unwhitenedError(cameras, point_);
else
return zero(myCameras.size() * 3);
return zero(cameras.size() * 3);
}
/**
@ -675,7 +555,7 @@ public:
}
if (this->degenerate_) {
return 0.0; // TODO: this maybe should be zero?
return 0.0; // TODO: this maybe should be zero?
// std::cout
// << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
// << std::endl;
@ -744,9 +624,9 @@ private:
};
/// traits
template<class CALIBRATION, size_t D>
struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > :
public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > {
template<class CALIBRATION>
struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionFactor<CALIBRATION> > {
};
} // \ namespace gtsam

23
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once
@ -38,7 +39,14 @@ namespace gtsam {
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION, 6> {
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
public:
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@ -50,7 +58,7 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<CALIBRATION, 6> Base;
typedef SmartStereoProjectionFactor<CALIBRATION> Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
@ -65,15 +73,16 @@ public:
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
SmartStereoProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
linearizeTo) {
}
/** Virtual destructor */
virtual ~SmartStereoProjectionPoseFactor() {}

122
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -18,8 +18,8 @@
* @date Sept 2013
*/
// TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
@ -32,8 +32,6 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
static bool isDebugTest = false;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w = 640, h = 480;
@ -66,7 +64,6 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -83,9 +80,10 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
return measurements_cam;
}
LevenbergMarquardtParams params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
fprintf(stderr, "Test 1 Complete");
SmartFactor::shared_ptr factor1(new SmartFactor());
}
@ -106,15 +104,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) {
factor1.add(measurement1, poseKey1, model, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) {
bool manageDegeneracy = true;
bool enableEPI = false;
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
body_P_sensor1);
factor1.add(measurement1, poseKey1, model, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Equals ) {
SmartFactor::shared_ptr factor1(new SmartFactor());
@ -128,8 +117,6 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
/* *************************************************************************/
TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
@ -169,8 +156,6 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, noisy ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
@ -226,10 +211,6 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
cout
<< " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************"
<< endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
@ -286,31 +267,31 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
values.insert(x2, pose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, pose3 * noise_pose);
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
params.verbosity = NonlinearOptimizerParams::ERROR;
EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
Values result;
gttic_ (SmartStereoProjectionPoseFactor);
gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor);
tictoc_finishedIteration_();
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
// VectorValues delta = GFG->optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
VectorValues delta = GFG->optimize();
VectorValues expected = VectorValues::Zero(delta);
EXPECT(assert_equal(expected, delta, 1e-6));
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
@ -345,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -373,7 +354,6 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
values.insert(x2, pose2);
values.insert(x3, pose3 * noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -414,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
@ -446,7 +426,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
values.insert(x3, pose3 * noise_pose);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -493,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);
@ -530,7 +509,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -567,13 +545,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -592,8 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(x2, pose2);
// values.insert(x3, pose3*noise_pose);
//
// LevenbergMarquardtParams params;
// Values result;
//// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// result = optimizer.optimize();
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@ -601,7 +578,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -653,15 +629,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(L(1), landmark1);
// values.insert(L(2), landmark2);
// values.insert(L(3), landmark3);
// if(isDebugTest) values.at<Pose3>(x3).print("Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// Values result = optimizer.optimize();
//
// if(isDebugTest) result.at<Pose3>(x3).print("Pose3 after optimization: ");
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//}
//
@ -723,8 +694,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
values.insert(x3, pose3 * noise_pose);
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
// TODO: next line throws Cheirality exception on Mac
boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
@ -749,7 +718,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
+ hessianFactor3->augmentedInformation();
// Check Information vector
// cout << AugInformationMatrix.size() << endl;
Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
// Check Hessian
@ -758,7 +726,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -811,11 +778,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2*noise_pose);
// // initialize third pose with some noise, we expect it to move back to original pose3
// values.insert(x3, pose3*noise_pose*noise_pose);
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
@ -825,15 +787,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// tictoc_finishedIteration_();
//
// // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_();
//}
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -894,11 +852,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2);
// // initialize third pose with some noise, we expect it to move back to original pose3
// values.insert(x3, pose3*noise_pose);
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
@ -908,15 +861,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// tictoc_finishedIteration_();
//
// // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_();
//}
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, Hessian ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -949,7 +898,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2);
//
// boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
// if(isDebugTest) hessianFactor->print("Hessian factor \n");
//
// // compute triangulation from linearization point
// // compute reprojection errors (sum squared)
@ -960,8 +908,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -1031,8 +977,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -1063,8 +1007,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values);
if (isDebugTest)
hessianFactor->print("Hessian factor \n");
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
@ -1075,8 +1017,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues);
if (isDebugTest)
hessianFactorRot->print("Hessian factor \n");
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(

10
gtsam/nonlinear/tests/testExpressionFactor.cpp → tests/testExpressionFactor.cpp
View File

@ -37,6 +37,10 @@ using namespace gtsam;
Point2 measured(-17, 30);
SharedNoiseModel model = noiseModel::Unit::Create(2);
// This deals with the overload problem and makes the expressions factor
// understand that we work on Point3
Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
namespace leaf {
// Create some values
struct MyValues: public Values {
@ -313,7 +317,7 @@ TEST(ExpressionFactor, tree) {
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ xy_hat(Project, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create factor and check value, dimension, linearization
@ -331,8 +335,6 @@ TEST(ExpressionFactor, tree) {
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
Expression<Point2>::TernaryFunction<Pose3, Point3, Cal3_S2>::type fff = project6;
// Try ternary version
ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
@ -489,7 +491,7 @@ TEST(ExpressionFactor, tree_finite_differences) {
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ xy_hat(Project, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
const double fd_step = 1e-5;

56
timing/DummyFactor.h Normal file
View File

@ -0,0 +1,56 @@
/**
* @file DummyFactor.h
* @brief Just to help in timing overhead
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/RegularImplicitSchurFactor.h>
namespace gtsam {
/**
* DummyFactor
*/
template<size_t D> //
class DummyFactor: public RegularImplicitSchurFactor<D> {
public:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
DummyFactor() {
}
DummyFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) :RegularImplicitSchurFactor<D>(Fblocks,E,P,b)
{
}
virtual ~DummyFactor() {
}
public:
/**
* @brief Dummy version to measure overhead of key access
*/
void multiplyHessian(double alpha, const VectorValues& x,
VectorValues& y) const {
BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) {
static const Vector empty;
Key key = Fi.first;
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
Vector& yi = it.first->second;
yi = x.at(key);
}
}
};
// DummyFactor
}

152
timing/timeSchurFactors.cpp Normal file
View File

@ -0,0 +1,152 @@
/**
* @file timeSchurFactors.cpp
* @brief Time various Schur-complement Jacobian factors
* @author Frank Dellaert
* @date Oct 27, 2013
*/
#include "DummyFactor.h"
#include <gtsam/base/timing.h>
#include <gtsam/slam/JacobianFactorQ.h>
#include "gtsam/slam/JacobianFactorQR.h"
#include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
#define SLOW
#define RAW
#define HESSIAN
#define NUM_ITERATIONS 1000
// Create CSV file for results
ofstream os("timeSchurFactors.csv");
/*************************************************************************************/
template<size_t D>
void timeAll(size_t m, size_t N) {
cout << m << endl;
// create F
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef pair<Key, Matrix2D> KeyMatrix2D;
vector < pair<Key, Matrix2D> > Fblocks;
for (size_t i = 0; i < m; i++)
Fblocks.push_back(KeyMatrix2D(i, (i + 1) * Matrix::Ones(2, D)));
// create E
Matrix E(2 * m, 3);
for (size_t i = 0; i < m; i++)
E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3);
// Calculate point covariance
Matrix P = (E.transpose() * E).inverse();
// RHS and sigmas
const Vector b = gtsam::repeat(2 * m, 1);
const SharedDiagonal model;
// parameters for multiplyHessianAdd
double alpha = 0.5;
VectorValues xvalues, yvalues;
for (size_t i = 0; i < m; i++)
xvalues.insert(i, gtsam::repeat(D, 2));
// Implicit
RegularImplicitSchurFactor<D> implicitFactor(Fblocks, E, P, b);
// JacobianFactor with same error
JacobianFactorQ<D, 2> jf(Fblocks, E, P, b, model);
// JacobianFactorQR with same error
JacobianFactorQR<D, 2> jqr(Fblocks, E, P, b, model);
// Hessian
HessianFactor hessianFactor(jqr);
#define TIME(label,factor,xx,yy) {\
for (size_t t = 0; t < N; t++) \
factor.multiplyHessianAdd(alpha, xx, yy);\
gttic_(label);\
for (size_t t = 0; t < N; t++) {\
factor.multiplyHessianAdd(alpha, xx, yy);\
}\
gttoc_(label);\
tictoc_getNode(timer, label)\
os << timer->secs()/NUM_ITERATIONS << ", ";\
}
#ifdef SLOW
TIME(Implicit, implicitFactor, xvalues, yvalues)
TIME(Jacobian, jf, xvalues, yvalues)
TIME(JacobianQR, jqr, xvalues, yvalues)
#endif
#ifdef HESSIAN
TIME(Hessian, hessianFactor, xvalues, yvalues)
#endif
#ifdef OVERHEAD
DummyFactor<D> dummy(Fblocks, E, P, b);
TIME(Overhead,dummy,xvalues,yvalues)
#endif
#ifdef RAW
{ // Raw memory Version
FastVector < Key > keys;
for (size_t i = 0; i < m; i++)
keys += i;
Vector x = xvalues.vector(keys);
double* xdata = x.data();
// create a y
Vector y = zero(m * D);
TIME(RawImplicit, implicitFactor, xdata, y.data())
TIME(RawJacobianQ, jf, xdata, y.data())
TIME(RawJacobianQR, jqr, xdata, y.data())
}
#endif
os << m << endl;
} // timeAll
/*************************************************************************************/
int main(void) {
#ifdef SLOW
os << "Implicit,";
os << "JacobianQ,";
os << "JacobianQR,";
#endif
#ifdef HESSIAN
os << "Hessian,";
#endif
#ifdef OVERHEAD
os << "Overhead,";
#endif
#ifdef RAW
os << "RawImplicit,";
os << "RawJacobianQ,";
os << "RawJacobianQR,";
#endif
os << "m" << endl;
// define images
vector < size_t > ms;
// ms += 2;
// ms += 3, 4, 5, 6, 7, 8, 9, 10;
// ms += 11,12,13,14,15,16,17,18,19;
// ms += 20, 30, 40, 50;
// ms += 20,30,40,50,60,70,80,90,100;
for (size_t m = 2; m <= 50; m += 2)
ms += m;
//for (size_t m=10;m<=100;m+=10) ms += m;
// loop over number of images
BOOST_FOREACH(size_t m,ms)
timeAll<6>(m, NUM_ITERATIONS);
}
//*************************************************************************************