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

Merge remote-tracking branch 'origin/develop' into feature/constraints 

Conflicts:
	gtsam/nonlinear/NonlinearEquality.h
release/4.3a0
dellaert 2014-11-26 11:59:00 +01:00
parent 64000bb747 846513cdb5
commit 6d2f47a98c
34 changed files with 2675 additions and 311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2777,6 +2777,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCustomChartExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

5
CMakeLists.txt
View File

@ -101,8 +101,6 @@ if(MSVC)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
# Also disable certain warnings
add_definitions(/wd4503)
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
@ -281,7 +279,7 @@ include_directories(BEFORE
if(MSVC)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@ -330,7 +328,6 @@ endif(GTSAM_BUILD_UNSTABLE)
# Install config and export files
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
message("GTSAM export: ${GTSAM_EXPORTED_TARGETS}")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)

3
examples/SFMExample_SmartFactor.cpp
View File

@ -61,8 +61,7 @@ using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3_S2> SmartFactor;
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
/* ************************************************************************* */
int main(int argc, char* argv[]) {

4
gtsam.h
View File

@ -2272,7 +2272,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, LANDMARK, CALIBRATION>
template<POSE, CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
@ -2288,7 +2288,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>

8
gtsam/CMakeLists.txt
View File

@ -10,7 +10,7 @@ set (gtsam_subdirs
linear
nonlinear
slam
navigation
navigation
)
set(gtsam_srcs)
@ -74,8 +74,8 @@ set(gtsam_srcs
${linear_srcs}
${nonlinear_srcs}
${slam_srcs}
${navigation_srcs}
${gtsam_core_headers}
${navigation_srcs}
${gtsam_core_headers}
)
# Generate and install config and dllexport files
@ -92,8 +92,6 @@ set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSIO
set(gtsam_soversion ${GTSAM_VERSION_MAJOR})
message(STATUS "GTSAM Version: ${gtsam_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
message("GTSAM Additional: ${GTSAM_ADDITIONAL_LIBRARIES}")
message("GTSAM Exports: ${GTSAM_EXPORTED_TARGETS}")
# build shared and static versions of the library
if (GTSAM_BUILD_STATIC_LIBRARY)

66
gtsam/base/numericalDerivative.h
View File

@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
delta);
}
// The benefit of this method is that it does not need to know what types are involved
// to evaluate the factor. If all the machinery of gtsam is working correctly, we should
// get the correct numerical derivatives out the other side.
template<typename FactorType>
JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor,
const Values& values,
double fd_step) {
Eigen::VectorXd e = factor.unwhitenedError(values);
const size_t rows = e.size();
std::map<Key, Matrix> jacobians;
typename FactorType::const_iterator key_it = factor.begin();
VectorValues dX = values.zeroVectors();
for(; key_it != factor.end(); ++key_it) {
size_t key = *key_it;
// Compute central differences using the values struct.
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for(size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = fd_step;
dX[key] = dx;
Values eval_values = values.retract(dX);
Eigen::VectorXd left = factor.unwhitenedError(eval_values);
dx[col] = -fd_step;
dX[key] = dx;
eval_values = values.retract(dX);
Eigen::VectorXd right = factor.unwhitenedError(eval_values);
J.col(col) = (left - right) * (1.0/(2.0 * fd_step));
}
jacobians[key] = J;
}
// Next step...return JacobianFactor
return JacobianFactor(jacobians, -e);
}
template<typename FactorType>
void testFactorJacobians(TestResult& result_,
const std::string& name_,
const FactorType& f,
const gtsam::Values& values,
double fd_step,
double tolerance) {
// Check linearization
JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian evalJ = jf->jacobianUnweighted();
Jacobian estJ = expected.jacobianUnweighted();
EXPECT(assert_equal(evalJ.first, estJ.first, tolerance));
EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
}
} // namespace gtsam
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }

5
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -52,6 +52,11 @@ namespace gtsam {
/// constructor from vector
Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b) :
K_(fov, w, h), b_(b) {
}
/// @}
/// @name Testable
/// @{

3
gtsam/geometry/StereoCamera.cpp
View File

@ -30,7 +30,8 @@ namespace gtsam {
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);

17
gtsam/geometry/StereoCamera.h
View File

@ -114,21 +114,18 @@ public:
/*
* project 3D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
/*
* to accomodate tsam's assumption that K is estimated, too
/**
*
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> H2) const {
return project(point, H1, H2);
}
Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);

10
gtsam/geometry/StereoPoint2.cpp
View File

@ -19,10 +19,18 @@
#include <gtsam/geometry/StereoPoint2.h>
using namespace std;
using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
void StereoPoint2::print(const string& s) const {
cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const StereoPoint2& p) {
os << '(' << p.uL() << ", " << p.uR() << ", " << p.v() << ')';
return os;
}
} // namespace gtsam

9
gtsam/geometry/StereoPoint2.h
View File

@ -88,7 +88,7 @@ namespace gtsam {
StereoPoint2 operator-(const StereoPoint2& b) const {
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
}
/// @}
/// @name Manifold
/// @{
@ -143,15 +143,18 @@ namespace gtsam {
}
/** convenient function to get a Point2 from the left image */
inline Point2 point2(){
Point2 point2() const {
return Point2(uL_, v_);
}
/** convenient function to get a Point2 from the right image */
inline Point2 right(){
Point2 right() const {
return Point2(uR_, v_);
}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
private:
/// @}

8
gtsam/nonlinear/Values-inl.h
View File

@ -302,8 +302,8 @@ namespace gtsam {
}
// overloaded insert with chart initializer
template<typename ValueType, typename Chart, typename ChartInit>
void Values::insert(Key j, const ValueType& val, ChartInit chart) {
template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val, Chart chart) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
@ -320,8 +320,8 @@ namespace gtsam {
}
// update with chart initializer, /todo: perhaps there is a way to init chart from old value...
template<typename ValueType, typename Chart, typename ChartInit>
void Values::update(Key j, const ValueType& val, ChartInit chart) {
template<typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val, Chart chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}

8
gtsam/nonlinear/Values.h
View File

@ -263,8 +263,8 @@ namespace gtsam {
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer
template <typename ValueType, typename Chart, typename ChartInit>
void insert(Key j, const ValueType& val, ChartInit chart);
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val, Chart chart);
/** insert that mimics the STL map insert - if the value already exists, the map is not modified
@ -288,8 +288,8 @@ namespace gtsam {
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart initializer
template <typename T, typename Chart, typename ChartInit>
void update(Key j, const T& val, ChartInit chart);
template <typename T, typename Chart>
void update(Key j, const T& val, Chart chart);
/** update the current available values without adding new ones */
void update(const Values& values);

16
gtsam/slam/JacobianFactorQ.h
View File

@ -12,10 +12,10 @@ namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQ: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base;
typedef JacobianSchurFactor<D, ZDim> Base;
public:
@ -25,7 +25,7 @@ public:
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
@ -40,8 +40,8 @@ public:
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
size_t j = 0, m2 = E.rows(), m = m2 / 2;
JacobianSchurFactor<D, ZDim>() {
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
@ -49,9 +49,9 @@ public:
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector < KeyMatrix > QF;
QF.reserve(m);
// Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D)
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model);
}

12
gtsam/slam/JacobianFactorQR.h
View File

@ -12,10 +12,10 @@ namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQR: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base;
typedef JacobianSchurFactor<D, ZDim> Base;
public:
@ -25,14 +25,14 @@ public:
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
JacobianSchurFactor<D, ZDim>() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
b.segment<2>(2 * i), model);
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model);
i += 1;
}
//gfg.print("gfg");

16
gtsam/slam/JacobianFactorSVD.h
View File

@ -11,12 +11,12 @@ namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorSVD: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
public:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
@ -25,7 +25,7 @@ public:
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
@ -37,9 +37,9 @@ public:
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
size_t numKeys = Enull.rows() / 2;
size_t j = 0, m2 = 2*numKeys-3;
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim*numKeys-3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
@ -48,7 +48,7 @@ public:
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second));
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
}
};

4
gtsam/slam/JacobianSchurFactor.h
View File

@ -18,12 +18,12 @@ namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
template<size_t D, size_t ZDim>
class JacobianSchurFactor: public JacobianFactor {
public:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// Use eigen magic to access raw memory

160
gtsam/slam/SmartFactorBase.h
View File

@ -19,16 +19,16 @@
#pragma once
#include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h"
#include "RegularImplicitSchurFactor.h"
#include "RegularHessianFactor.h"
#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/geometry/PinholeCamera.h>
#include <gtsam/geometry/PinholeCamera.h> // for Cheirality exception
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>
@ -36,40 +36,48 @@
namespace gtsam {
/// Base class with no internal point, completely functional
template<class POSE, class CALIBRATION, size_t D>
template<class POSE, class Z, class CAMERA, size_t D>
class SmartFactorBase: public NonlinearFactor {
protected:
// Keep a copy of measurement and calibration for I/O
std::vector<Point2> measured_; ///< 2D measurement for each of the m views
std::vector<Z> measured_; ///< 2D measurement for each of the m views
std::vector<SharedNoiseModel> noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
static const int ZDim = traits::dimension<Z>::value; ///< Measurement dimension
/// Definitions for blocks of F
typedef Eigen::Matrix<double, 2, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, 2> MatrixD2; // 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
typedef Eigen::Matrix<double, 2, 3> Matrix23;
typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, 2, 2> Matrix2;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
/// shorthand for base class type
typedef NonlinearFactor Base;
/// shorthand for this class
typedef SmartFactorBase<POSE, CALIBRATION, D> This;
typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// 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 std::vector<Camera> Cameras;
typedef CAMERA Camera;
typedef std::vector<CAMERA> Cameras;
/**
* Constructor
@ -89,7 +97,7 @@ public:
* @param poseKey is the index corresponding to the camera observing the landmark
* @param noise_i is the measurement noise
*/
void add(const Point2& measured_i, const Key& poseKey_i,
void add(const Z& measured_i, const Key& poseKey_i,
const SharedNoiseModel& noise_i) {
this->measured_.push_back(measured_i);
this->keys_.push_back(poseKey_i);
@ -100,7 +108,7 @@ public:
* variant of the previous add: adds a bunch of measurements, together with the camera keys and noises
*/
// ****************************************************************************************************
void add(std::vector<Point2>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
std::vector<SharedNoiseModel>& noises) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
@ -113,7 +121,7 @@ public:
* variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them
*/
// ****************************************************************************************************
void add(std::vector<Point2>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
@ -127,7 +135,8 @@ public:
* The noise is assumed to be the same for all measurements
*/
// ****************************************************************************************************
void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) {
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
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);
@ -136,7 +145,7 @@ public:
}
/** return the measurements */
const std::vector<Point2>& measured() const {
const std::vector<Z>& measured() const {
return measured_;
}
@ -179,18 +188,18 @@ public:
}
// ****************************************************************************************************
/// Calculate vector of re-projection errors, before applying noise model
// /// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
Vector b = zero(2 * cameras.size());
Vector b = zero(ZDim * cameras.size());
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
const Point2& zi = this->measured_.at(i);
BOOST_FOREACH(const CAMERA& camera, cameras) {
const Z& zi = this->measured_.at(i);
try {
Point2 e(camera.project(point) - zi);
b[2 * i] = e.x();
b[2 * i + 1] = e.y();
Z e(camera.project(point) - zi);
b[ZDim * i] = e.x();
b[ZDim * i + 1] = e.y();
} catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE);
@ -215,10 +224,10 @@ public:
double overallError = 0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
const Point2& zi = this->measured_.at(i);
BOOST_FOREACH(const CAMERA& camera, cameras) {
const Z& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point) - zi);
Z reprojectionError(camera.project(point) - zi);
overallError += 0.5
* this->noise_.at(i)->distance(reprojectionError.vector());
} catch (CheiralityException&) {
@ -236,19 +245,19 @@ public:
const Point3& point) const {
int numKeys = this->keys_.size();
E = zeros(2 * numKeys, 3);
E = zeros(ZDim * numKeys, 3);
Vector b = zero(2 * numKeys);
Matrix Ei(2, 3);
Matrix Ei(ZDim, 3);
for (size_t i = 0; i < this->measured_.size(); i++) {
try {
cameras[i].project(point, boost::none, Ei);
cameras[i].project(point, boost::none, Ei, boost::none);
} catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE);
}
this->noise_.at(i)->WhitenSystem(Ei, b);
E.block<2, 3>(2 * i, 0) = Ei;
E.block<ZDim, 3>(ZDim * i, 0) = Ei;
}
// Matrix PointCov;
@ -262,11 +271,11 @@ public:
Vector& b, const Cameras& cameras, const Point3& point) const {
size_t numKeys = this->keys_.size();
E = zeros(2 * numKeys, 3);
b = zero(2 * numKeys);
E = zeros(ZDim * numKeys, 3);
b = zero(ZDim * numKeys);
double f = 0;
Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D);
Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D);
for (size_t i = 0; i < this->measured_.size(); i++) {
Vector bi;
@ -288,12 +297,12 @@ public:
if (D == 6) { // optimize only camera pose
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
} else {
Hcam.block<2, 6>(0, 0) = Fi; // 2 x 6 block for the cameras
Hcam.block<2, D - 6>(0, 6) = Hcali; // 2 x nrCal block for the cameras
Hcam.block<ZDim, 6>(0, 0) = Fi; // ZDim x 6 block for the cameras
Hcam.block<ZDim, D - 6>(0, 6) = Hcali; // ZDim x nrCal block for the cameras
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam));
}
E.block<2, 3>(2 * i, 0) = Ei;
subInsert(b, bi, 2 * i);
E.block<ZDim, 3>(ZDim * i, 0) = Ei;
subInsert(b, bi, ZDim * i);
}
return f;
}
@ -334,10 +343,10 @@ public:
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
F = zeros(2 * numKeys, D * numKeys);
F = zeros(This::ZDim * numKeys, D * numKeys);
for (size_t i = 0; i < this->keys_.size(); ++i) {
F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
}
return f;
}
@ -356,9 +365,9 @@ public:
// Do SVD on A
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
// Enull = zeros(2 * numKeys, 2 * numKeys - 3);
// Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3);
size_t numKeys = this->keys_.size();
Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns
Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns
return f;
}
@ -372,11 +381,11 @@ public:
int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(2 * numKeys, D * numKeys);
F.resize(ZDim * numKeys, D * numKeys);
F.setZero();
for (size_t i = 0; i < this->keys_.size(); ++i)
F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
return f;
}
@ -437,9 +446,9 @@ public:
int numKeys = this->keys_.size();
/// Compute Full F ????
Matrix F = zeros(2 * numKeys, D * numKeys);
Matrix F = zeros(ZDim * numKeys, D * numKeys);
for (size_t i = 0; i < this->keys_.size(); ++i)
F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
Matrix H(D * numKeys, D * numKeys);
Vector gs_vector;
@ -477,16 +486,16 @@ public:
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
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<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
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) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
* (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
@ -494,7 +503,7 @@ public:
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
@ -521,16 +530,16 @@ public:
// X X X X 14
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
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<2>(2 * i1) // F' * b
-Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
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) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
* (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
GsIndex++;
}
// upper triangular part of the hessian
@ -539,7 +548,7 @@ public:
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
GsIndex++;
}
} // end of for over cameras
@ -587,9 +596,9 @@ public:
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<2, 3>(2 * i1, 0) * P;
const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// D = (Dx2) * (2)
// 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];
@ -599,15 +608,15 @@ public:
// 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<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+ 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) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// (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<2, 3>(2 * i1, 0).transpose() * Fi1) );
( 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
@ -616,12 +625,12 @@ public:
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
// (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<2, 3>(2 * i2, 0).transpose() * Fi2);
- Fi1.transpose() * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
@ -641,7 +650,7 @@ public:
}
// ****************************************************************************************************
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks;
@ -650,18 +659,18 @@ public:
Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
return boost::make_shared<JacobianFactorQ<D> >(Fblocks, E, PointCov, b);
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b);
}
// ****************************************************************************************************
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;
std::vector<KeyMatrix2D> Fblocks;
Vector b;
Matrix Enull(2*numKeys, 2*numKeys-3);
Matrix Enull(ZDim*numKeys, ZDim*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b);
return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
}
private:
@ -676,4 +685,7 @@ private:
}
};
template<class POSE, class Z, class CAMERA, size_t D>
const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim;
} // \ namespace gtsam

25
gtsam/slam/SmartProjectionFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include "SmartFactorBase.h"
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
@ -61,8 +61,8 @@ enum LinearizationMode {
* SmartProjectionFactor: triangulates point
* TODO: why LANDMARK parameter?
*/
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<POSE, CALIBRATION, D> {
template<class POSE, class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
protected:
// Some triangulation parameters
@ -92,7 +92,7 @@ protected:
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<POSE, CALIBRATION, D> Base;
typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -102,7 +102,9 @@ protected:
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef SmartProjectionFactor<POSE, CALIBRATION, D> This;
static const int ZDim = traits::dimension<Point2>::value; ///< Measurement dimension
public:
@ -418,16 +420,16 @@ public:
}
/// create factor
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<D, ZDim> > 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<D, ZDim> >(this->keys_);
}
/// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
const Values& values, double lambda) const {
Cameras myCameras;
// TODO triangulate twice ??
@ -435,7 +437,7 @@ public:
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
else
return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
}
/// different (faster) way to compute Jacobian factor
@ -444,7 +446,7 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
return boost::make_shared< JacobianFactorSVD<D> >(this->keys_);
return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
}
/// Returns true if nonDegenerate
@ -707,4 +709,7 @@ private:
}
};
template<class POSE, class CALIBRATION, size_t D>
const int SmartProjectionFactor<POSE, CALIBRATION, D>::ZDim;
} // \ namespace gtsam

10
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include "SmartProjectionFactor.h"
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam {
/**
@ -37,8 +37,8 @@ namespace gtsam {
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
template<class POSE, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, CALIBRATION, 6> {
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@ -48,10 +48,10 @@ protected:
public:
/// shorthand for base class type
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
typedef SmartProjectionFactor<POSE, CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;
typedef SmartProjectionPoseFactor<POSE, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;

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

@ -114,7 +114,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactor with same error
const SharedDiagonal model;
JacobianFactorQ<6> jf(Fblocks, E, P, b, model);
JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
{ // error
double expectedError = jf.error(xvalues);
@ -164,7 +164,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
}
// Create JacobianFactorQR
JacobianFactorQR<6> jfq(Fblocks, E, P, b, model);
JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
{
const SharedDiagonal model;
VectorValues yActual = zero;

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

@ -60,8 +60,8 @@ static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
typedef SmartProjectionPoseFactor<Pose3,Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Pose3,Cal3Bundler> SmartFactorBundler;
void projectToMultipleCameras(
SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
SmartProjectionPoseFactor<Pose3,Cal3Bundler> factor1(rankTol, linThreshold);
boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
factor1.add(measurement1, poseKey1, model, Kbundler);
}

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -46,7 +46,7 @@ using namespace gtsam;
int main(int argc, char** argv){
typedef SmartProjectionPoseFactor<Pose3, Point3, Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Pose3, Cal3_S2> SmartFactor;
Values initial_estimate;
NonlinearFactorGraph graph;

153
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp Normal file
View File

@ -0,0 +1,153 @@
/* ----------------------------------------------------------------------------
* 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 SmartProjectionFactorExample.cpp
* @brief A stereo visual odometry example
* @date May 30, 2014
* @author Stephen Camp
* @author Chris Beall
*/
/**
* A smart projection factor example based on stereo data, throwing away the
* measurement from the right camera
* -robot starts at origin
* -moves forward, taking periodic stereo measurements
* -makes monocular observations of many landmarks
*/
#include <gtsam/geometry/Pose3.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_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <string>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor;
bool output_poses = true;
bool output_initial_poses = true;
string poseOutput("../../../examples/data/optimized_poses.txt");
string init_poseOutput("../../../examples/data/initial_poses.txt");
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
ofstream pose3Out, init_pose3Out;
bool add_initial_noise = true;
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
cout << "Reading calibration info" << endl;
ifstream calibration_file(calibration_loc.c_str());
double fx, fy, s, u0, v0, b;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b));
cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str());
int pose_id;
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++) {
pose_file >> m.data()[i];
}
if(add_initial_noise){
m(1,3) += (pose_id % 10)/10.0;
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
Values initial_pose_values = initial_estimate.filter<Pose3>();
if(output_poses){
init_pose3Out.open(init_poseOutput.c_str(),ios::out);
for(int i = 1; i<=initial_pose_values.size(); i++){
init_pose3Out << i << " " << initial_pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}
}
// camera and landmark keys
size_t x, 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
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor());
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor());
current_l = l;
}
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',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));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
if(output_poses){
pose3Out.open(poseOutput.c_str(),ios::out);
for(int i = 1; i<=pose_values.size(); i++){
pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}
cout << "Writing output" << endl;
}
return 0;
}

8
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -252,7 +252,7 @@ public:
}
/// Return dimensions for each argument, as a map
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
}
// Return size needed for memory buffer in traceExecution
@ -324,7 +324,7 @@ public:
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
// get dimension from the chart; only works for fixed dimension charts
map[key_] = traits::dimension<Chart>::value;
}
@ -371,7 +371,7 @@ public:
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
map[key_] = traits::dimension<T>::value;
}
@ -523,7 +523,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
Base::dims(map);
This::expression->dims(map);
}

115
gtsam_unstable/nonlinear/Expression.h
View File

@ -20,17 +20,31 @@
#pragma once
#include "Expression-inl.h"
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Symbol.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
class ExpressionFactorShallowTest;
namespace gtsam {
// Forward declare
template<typename T> class ExpressionFactor;
/**
* Expression class that supports automatic differentiation
*/
template<typename T>
class Expression {
public:
/// Define type so we can apply it as a meta-function
typedef Expression<T> type;
private:
// Paul's trick shared pointer, polymorphic root of entire expression tree
@ -55,7 +69,7 @@ public:
// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))) {
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a nullary method expression
@ -87,8 +101,7 @@ public:
template<typename A1, typename A2>
Expression(typename BinaryExpression<T, A1, A2>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
/// Construct a ternary function expression
@ -101,14 +114,9 @@ public:
expression2, expression3)) {
}
/// Return keys that play in this expression
std::set<Key> keys() const {
return root_->keys();
}
/// Return dimensions for each argument, as a map
void dims(std::map<Key, size_t>& map) const {
root_->dims(map);
/// Return root
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
// Return size needed for memory buffer in traceExecution
@ -116,13 +124,81 @@ public:
return root_->traceSize();
}
/// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below!
/// Return keys that play in this expression
std::set<Key> keys() const {
return root_->keys();
}
/// Return dimensions for each argument, as a map
void dims(std::map<Key, int>& map) const {
root_->dims(map);
}
/**
* @brief Return value and optional derivatives, reverse AD version
* Notes: this is not terribly efficient, and H should have correct size.
* The order of the Jacobians is same as keys in either keys() or dims()
*/
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const {
if (H) {
// Call private version that returns derivatives in H
KeysAndDims pair = keysAndDims();
return value(values, pair.first, pair.second, *H);
} else
// no derivatives needed, just return value
return root_->value(values);
}
private:
/// Vaguely unsafe keys and dimensions in same order
typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
KeysAndDims keysAndDims() const {
std::map<Key, int> map;
dims(map);
size_t n = map.size();
KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
boost::copy(map | boost::adaptors::map_values, pair.second.begin());
return pair;
}
/// private version that takes keys and dimensions, returns derivatives
T value(const Values& values, const FastVector<Key>& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H->size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits::dimension<T>::value;
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
JacobianMap jacobianMap(keys, Ab);
// Call unsafe version
T result = value(values, jacobianMap);
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
return result;
}
/// trace execution, very unsafe
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return root_->traceExecution(values, trace, traceStorage);
}
/// Return value and derivatives, reverse AD version
/**
* @brief Return value and derivatives, reverse AD version
* This very unsafe method needs a JacobianMap with correctly allocated
* and initialized VerticalBlockMatrix, hence is declared private.
*/
T value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
@ -137,17 +213,10 @@ public:
return value;
}
/// Return value
T value(const Values& values) const {
return root_->value(values);
}
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
friend class ::ExpressionFactorShallowTest;
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
/// Define type so we can apply it as a meta-function
typedef Expression<T> type;
};
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator

58
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -22,8 +22,6 @@
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
#include <numeric>
namespace gtsam {
@ -36,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor {
T measurement_; ///< the measurement to be compared with the expression
Expression<T> expression_; ///< the expression that is AD enabled
std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
static const int Dim = traits::dimension<T>::value;
@ -54,23 +52,15 @@ public:
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
// Get dimensions of Jacobian matrices
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
std::map<Key, size_t> map;
expression_.dims(map);
size_t n = map.size();
keys_.resize(n);
boost::copy(map | boost::adaptors::map_keys, keys_.begin());
dimensions_.resize(n);
boost::copy(map | boost::adaptors::map_values, dimensions_.begin());
boost::tie(keys_, dims_) = expression_.keysAndDims();
// Add sizes to know how much memory to allocate on stack in linearize
augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1);
augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1);
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
BOOST_FOREACH(size_t d, dims_)
std::cout << d << " ";
std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
#endif
@ -83,35 +73,17 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) {
// H should be pre-allocated
assert(H->size()==size());
VerticalBlockMatrix Ab(dimensions_, Dim);
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
JacobianMap map(keys_, Ab);
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
H->at(i) = Ab(i);
const T value = expression_.value(x, keys_, dims_, *H);
return chart.local(measurement_, value);
} else {
const T& value = expression_.value(x);
const T value = expression_.value(x);
return chart.local(measurement_, value);
}
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
@ -120,24 +92,28 @@ public:
// In case noise model is constrained, we need to provide a noise model
bool constrained = noiseModel_->is_constrained();
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dimensions_, Dim,
constrained ? new JacobianFactor(keys_, dims_, Dim,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dimensions_, Dim));
new JacobianFactor(keys_, dims_, Dim));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
JacobianMap map(keys_, Ab);
JacobianMap jacobianMap(keys_, Ab);
// Zero out Jacobian so we can simply add to it
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Get value and Jacobians, writing directly into JacobianFactor
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
Ab(size()).col(0) = -chart.local(measurement_, value);
// Whiten the corresponding system, Ab already contains RHS
Vector dummy(Dim);
noiseModel_->WhitenSystem(Ab.matrix(),dummy);
noiseModel_->WhitenSystem(Ab.matrix(), dummy);
return factor;
}

87
gtsam_unstable/nonlinear/expressionTesting.h
View File

@ -30,19 +30,88 @@
namespace gtsam {
/**
* Linearize a nonlinear factor using numerical differentiation
* The benefit of this method is that it does not need to know what types are
* involved to evaluate the factor. If all the machinery of gtsam is working
* correctly, we should get the correct numerical derivatives out the other side.
*/
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) {
// We will fill a map of Jacobians
std::map<Key, Matrix> jacobians;
// Get size
Eigen::VectorXd e = factor.whitenedError(values);
const size_t rows = e.size();
// Loop over all variables
VectorValues dX = values.zeroVectors();
BOOST_FOREACH(Key key, factor) {
// Compute central differences using the values struct.
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = delta;
dX[key] = dx;
Values eval_values = values.retract(dX);
Eigen::VectorXd left = factor.whitenedError(eval_values);
dx[col] = -delta;
dX[key] = dx;
eval_values = values.retract(dX);
Eigen::VectorXd right = factor.whitenedError(eval_values);
J.col(col) = (left - right) * (1.0 / (2.0 * delta));
}
jacobians[key] = J;
}
// Next step...return JacobianFactor
return JacobianFactor(jacobians, -e);
}
namespace internal {
// CPPUnitLite-style test for linearization of a factor
void testFactorJacobians(TestResult& result_, const std::string& name_,
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
// Check cast result and then equality
CHECK(actual);
EXPECT( assert_equal(expected, *actual, tolerance));
}
}
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }
namespace internal {
// CPPUnitLite-style test for linearization of an ExpressionFactor
template<typename T>
void testExpressionJacobians(TestResult& result_,
const std::string& name_,
const gtsam::Expression<T>& expression,
const gtsam::Values& values,
double nd_step,
double tolerance) {
void testExpressionJacobians(TestResult& result_, const std::string& name_,
const gtsam::Expression<T>& expression, const gtsam::Values& values,
double nd_step, double tolerance) {
// Create factor
size_t size = traits::dimension<T>::value;
ExpressionFactor<T> f(noiseModel::Unit::Create(size), expression.value(values), expression);
ExpressionFactor<T> f(noiseModel::Unit::Create(size),
expression.value(values), expression);
testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
}
} // namespace gtsam
}
} // namespace gtsam
/// \brief Check the Jacobians produced by an expression against finite differences.
/// \param expression The expression to test.
@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_,
/// \param numerical_derivative_step The step to use when computing the finite difference Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \
{ gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }
{ gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }

118
gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp Normal file
View File

@ -0,0 +1,118 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------1------------------------------------------- */
/**
* @file testCustomChartExpression.cpp
* @date September 18, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/nonlinear/expressionTesting.h>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
// A simple prototype custom chart that does two things:
// 1. it reduces the dimension of the variable being estimated
// 2. it scales the input to retract.
//
// The Jacobian of this chart is:
// [ 2 0 ]
// [ 0 3 ]
// [ 0 0 ]
struct ProjectionChart {
typedef Eigen::Vector3d type;
typedef Eigen::Vector2d vector;
static vector local(const type& origin, const type& other) {
return (other - origin).head<2>();
}
static type retract(const type& origin, const vector& d) {
return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0);
}
static int getDimension(const type& origin) {
return 2;
}
};
namespace gtsam {
namespace traits {
template<> struct is_chart<ProjectionChart> : public boost::true_type {};
template<> struct dimension<ProjectionChart> : public boost::integral_constant<int, 2> {};
} // namespace traits
} // namespace gtsam
TEST(ExpressionCustomChart, projection) {
// Create expression
Expression<Eigen::Vector3d> point(1);
Eigen::Vector3d pval(1.0, 2.0, 3.0);
Values standard;
standard.insert(1, pval);
Values custom;
custom.insert(1, pval, ProjectionChart());
Eigen::Vector3d pstandard = point.value(standard);
Eigen::Vector3d pcustom = point.value(custom);
// The values should be the same.
EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10));
EXPECT_LONGS_EQUAL(3, standard.at(1).dim());
VectorValues vstandard = standard.zeroVectors();
EXPECT_LONGS_EQUAL(3, vstandard.at(1).size());
EXPECT_LONGS_EQUAL(2, custom.at(1).dim());
VectorValues vcustom = custom.zeroVectors();
EXPECT_LONGS_EQUAL(2, vcustom.at(1).size());
ExpressionFactor<Eigen::Vector3d> f(noiseModel::Unit::Create(pval.size()), pval, point);
boost::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
boost::shared_ptr<JacobianFactor> jfstandard = //
boost::dynamic_pointer_cast<JacobianFactor>(gfstandard);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian Jstandard = jfstandard->jacobianUnweighted();
EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
boost::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
boost::shared_ptr<JacobianFactor> jfcustom = //
boost::dynamic_pointer_cast<JacobianFactor>(gfcustom);
Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
expectedJacobian(0,0) = 2.0;
expectedJacobian(1,1) = 3.0;
Jacobian Jcustom = jfcustom->jacobianUnweighted();
EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10));
// Amazingly, the finite differences get the expected Jacobian right.
const double fd_step = 1e-5;
const double tolerance = 1e-5;
EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

23
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -114,22 +114,20 @@ TEST(Expression, NullaryMethod) {
Values values;
values.insert(67, Point3(3, 4, 5));
// Pre-allocate JacobianMap
FastVector<Key> keys;
keys.push_back(67);
FastVector<int> dims;
dims.push_back(3);
VerticalBlockMatrix Ab(dims, 1);
JacobianMap map(keys, Ab);
// Check dims as map
std::map<Key, int> map;
norm.dims(map);
LONGS_EQUAL(1,map.size());
// Get value and Jacobian
double actual = norm.value(values, map);
// Get value and Jacobians
std::vector<Matrix> H(1);
double actual = norm.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
EXPECT(assert_equal(expected,Ab(0)));
EXPECT(assert_equal(expected,H[0]));
}
/* ************************************************************************* */
// Binary(Leaf,Leaf)
@ -159,7 +157,7 @@ TEST(Expression, BinaryKeys) {
/* ************************************************************************* */
// dimensions
TEST(Expression, BinaryDimensions) {
map<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3);
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
EXPECT(actual==expected);
}
@ -190,8 +188,7 @@ TEST(Expression, TreeKeys) {
/* ************************************************************************* */
// dimensions
TEST(Expression, TreeDimensions) {
map<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3)(3,
5);
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
tree::uv_hat.dims(actual);
EXPECT(actual==expected);
}

11
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -202,6 +202,17 @@ TEST(ExpressionFactor, Shallow) {
// Construct expression, concise evrsion
Point2_ expression = project(transform_to(x_, p_));
// Get and check keys and dims
FastVector<Key> keys;
FastVector<int> dims;
boost::tie(keys, dims) = expression.keysAndDims();
LONGS_EQUAL(2,keys.size());
LONGS_EQUAL(2,dims.size());
LONGS_EQUAL(1,keys[0]);
LONGS_EQUAL(2,keys[1]);
LONGS_EQUAL(6,dims[0]);
LONGS_EQUAL(3,dims[1]);
// traceExecution of shallow tree
typedef UnaryExpression<Point2, Point3> Unary;
typedef BinaryExpression<Point3, Pose3, Point3> Binary;

748
gtsam_unstable/slam/SmartStereoProjectionFactor.h Normal file
View File

@ -0,0 +1,748 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionFactor.h
* @brief Base class to create smart factors on poses or cameras
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
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
* TODO: why LANDMARK parameter?
*/
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
protected:
// Some triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
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
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<SmartStereoProjectionFactorState> state_;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::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
/// shorthand for this class
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef traits::dimension<gtsam::StereoPoint2> ZDim_t; ///< Dimension trait of measurement type
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a StereoCamera // TODO: Get rid of this?
typedef StereoCamera Camera;
/// Vector of cameras
typedef std::vector<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 body to sensor frame (default identity)
*/
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> 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_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state),
landmarkDistanceThreshold_(landmarkDistanceThreshold),
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
}
/** Virtual destructor */
virtual ~SmartStereoProjectionFactor() {
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
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 << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl;
Base::print("", keyFormatter);
}
/// 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
// 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_
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 (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true;
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
}
}
if (retriangulate) { // we store the current poses used for triangulation
cameraPosesTriangulation_.clear();
cameraPosesTriangulation_.reserve(m);
for (size_t i = 0; i < m; i++)
// cameraPosesTriangulation_[i] = cameras[i].pose();
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
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
}
/// triangulateSafe
size_t 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;
}
bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
//TODO: Chris will replace this with something else for stereo
// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
// rankTolerance_, enableEPI_);
// // // Temporary hack to use monocular triangulation
std::vector<Point2> mono_measurements;
BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
mono_measurements.push_back(sp.point2());
}
std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
BOOST_FOREACH(const Camera& camera, cameras) {
const Pose3& pose = camera.pose();
const Cal3_S2& K = camera.calibration()->calibration();
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
}
point_ = triangulatePoint3<Cal3_S2>(mono_cameras, mono_measurements,
rankTolerance_, enableEPI_);
// // // End temporary hack
// FIXME: temporary: triangulation using only first camera
// const StereoPoint2& z0 = this->measured_.at(0);
// point_ = cameras[0].backproject(z0);
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 StereoPoint2& zi = this->measured_.at(i);
try {
StereoPoint2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// 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;
}
/// 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 << "createImplicitSchurFactor: 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;
}
}
/// 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 {
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);
if (this->measured_.size() != cameras.size()) {
std::cout
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< std::endl;
exit(1);
}
this->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;
BOOST_FOREACH(gtsam::Matrix& m, Gs)
m = zeros(D, D);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
return boost::make_shared<RegularHessianFactor<D> >(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;
}
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
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;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
// 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;
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
gs_vector.noalias() = F.transpose()
* (b - (E * (PointCov * (E.transpose() * b))));
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++) {
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);
}
// // create factor
// boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createImplicitSchurFactor(cameras, point_, lambda);
// else
// return boost::shared_ptr<ImplicitSchurFactor<D> >();
// }
//
// /// create factor
// boost::shared_ptr<JacobianFactorQ<D> > 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_);
// }
//
// /// Create a factor, takes values
// boost::shared_ptr<JacobianFactorQ<D> > 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> >(this->keys_);
// }
//
/// 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);
else
return boost::make_shared< JacobianFactorSVD<D, ZDim_t::value> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& myCameras) 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);
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 << "SmartStereoProjectionFactor: this is not ready" << std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
}
return true;
}
/// Takes values
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate)
computeEP(E, PointCov, myCameras);
return nonDegenerate;
}
/// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
return Base::computeEP(E, PointCov, cameras, point_);
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const {
Cameras myCameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, PointCov, b, myCameras, 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,
Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!");
// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
// std::cout << "point " << point_ << std::endl;
// std::cout
// << "SmartStereoProjectionFactor: 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;
} else {
// nondegenerate: just return Base version
return 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, Matrix3& PointCov, Vector& b,
const Cameras& cameras, const double lambda) const {
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
}
/// 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);
if (nonDegenerate)
return reprojectionError(myCameras);
else
return zero(myCameras.size() * 3);
}
/**
* 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.
*/
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 (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
// 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 StereoPoint2& zi = this->measured_.at(i);
// if (i == 0) // first pose
// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
// StereoPoint2 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_);
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return chirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const {
return throwCheirality_;
}
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);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

218
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h Normal file
View File

@ -0,0 +1,218 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionPoseFactor.h
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
*/
#pragma once
#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
/**
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
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)
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, 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 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<POSE> 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) {}
/** Virtual destructor */
virtual ~SmartStereoProjectionPoseFactor() {}
/**
* 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 StereoPoint2 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<StereoPoint2> 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<StereoPoint2> 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);
}
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, 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);
}
/// get the dimension of the factor
virtual size_t dim() const {
return 6 * this->keys_.size();
}
/**
* 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);
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 :
throw("JacobianQ not working yet!");
// return this->createJacobianQFactor(cameras(values), 0.0);
break;
default:
return this->createHessianFactor(cameras(values));
break;
}
}
/**
* error calculates the error of the factor.
*/
virtual double error(const Values& values) const {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
/** return the calibration object */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
return K_all_;
}
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);
ar & BOOST_SERIALIZATION_NVP(K_all_);
}
}; // end of class declaration
} // \ namespace gtsam

1038
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp Normal file
View File

File diff suppressed because it is too large Load Diff