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

Merged in feature/LinearSolverCleanup (pull request #148) 

This has
- Separate Scatter class
- change from updateATA to updateHessian as a virtual method
- Fixed-size BinaryJacobianFactor that overloads updateHessian
- SFM Timing Script that takes BAL files
release/4.3a0
Frank Dellaert 2015-06-15 01:08:02 -07:00
parent 51433c1ec2 33e412f2ee
commit b68f763fe7
24 changed files with 1363 additions and 907 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/ClusterTree-inst.h
View File

@ -99,7 +99,7 @@ namespace gtsam
// Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, Ordering(node->keys));
eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
myData.bayesTreeNode->setEliminationResult(eliminationResult);
@ -123,7 +123,7 @@ namespace gtsam
const std::string& s, const KeyFormatter& keyFormatter) const
{
std::cout << s;
BOOST_FOREACH(Key j, keys)
BOOST_FOREACH(Key j, orderedFrontalKeys)
std::cout << j << " ";
std::cout << "problemSize = " << problemSize_ << std::endl;
}

6
gtsam/inference/ClusterTree.h
View File

@ -11,7 +11,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
namespace gtsam
{
@ -37,11 +37,11 @@ namespace gtsam
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
struct Cluster {
typedef FastVector<Key> Keys;
typedef Ordering Keys;
typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Cluster> > Children;
Keys keys; ///< Frontal keys of this node
Keys orderedFrontalKeys; ///< Frontal keys of this node
Factors factors; ///< Factors associated with this node
Children children; ///< sub-trees
int problemSize_;

17
gtsam/inference/JunctionTree-inst.h
View File

@ -49,7 +49,7 @@ namespace gtsam {
// structure with its own JT node, and create a child pointer in its parent.
ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
myData.myJTNode = boost::make_shared<typename JunctionTree<BAYESTREE,GRAPH>::Node>();
myData.myJTNode->keys.push_back(node->key);
myData.myJTNode->orderedFrontalKeys.push_back(node->key);
myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
parentData.myJTNode->children.push_back(myData.myJTNode);
return myData;
@ -101,13 +101,20 @@ namespace gtsam {
const typename JunctionTree<BAYESTREE, GRAPH>::Node& childToMerge =
*myData.myJTNode->children[child - nrMergedChildren];
// Merge keys, factors, and children.
myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end());
myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
myData.myJTNode->orderedFrontalKeys.insert(
myData.myJTNode->orderedFrontalKeys.begin(),
childToMerge.orderedFrontalKeys.begin(),
childToMerge.orderedFrontalKeys.end());
myData.myJTNode->factors.insert(myData.myJTNode->factors.end(),
childToMerge.factors.begin(),
childToMerge.factors.end());
myData.myJTNode->children.insert(myData.myJTNode->children.end(),
childToMerge.children.begin(),
childToMerge.children.end());
// Increment problem size
combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
// Increment number of frontal variables
myNrFrontals += childToMerge.keys.size();
myNrFrontals += childToMerge.orderedFrontalKeys.size();
// Remove child from list.
myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren));
// Increment number of merged children

18
gtsam/inference/Ordering.h
View File

@ -18,15 +18,15 @@
#pragma once
#include <algorithm>
#include <vector>
#include <boost/assign/list_inserter.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/MetisIndex.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/FastSet.h>
#include <boost/assign/list_inserter.hpp>
#include <algorithm>
#include <vector>
namespace gtsam {
@ -104,9 +104,9 @@ namespace gtsam {
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
/// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
/// constrainFirst will be ordered in the same order specified in the vector<Key> \c
/// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be
/// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
@ -117,7 +117,7 @@ namespace gtsam {
/// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the
/// variables in \c constrainFirst will be ordered in the same order specified in the
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
/// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well.
static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder = false);

91
gtsam/linear/BinaryJacobianFactor.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* 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 BinaryJacobianFactor.h
*
* @brief A binary JacobianFactor specialization that uses fixed matrix math for speed
*
* @date June 2015
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/timing.h>
namespace gtsam {
/**
* A binary JacobianFactor specialization that uses fixed matrix math for speed
*/
template<int M, int N1, int N2>
struct BinaryJacobianFactor: JacobianFactor {
/// Constructor
BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
Key key2, const Eigen::Matrix<double, M, N2>& A2,
const Eigen::Matrix<double, M, 1>& b, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(key1, A1, key2, A2, b, model) {
}
inline Key key1() const {
return keys_[0];
}
inline Key key2() const {
return keys_[1];
}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_BinaryJacobianFactor);
// Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) {
if (model->isConstrained())
throw std::invalid_argument(
"BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
key2(), model->Whiten(getA(end())), model->whiten(getb()));
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slot1 = Slot(infoKeys, key1());
DenseIndex slot2 = Slot(infoKeys, key2());
DenseIndex slotB = info->nBlocks() - 1;
const Matrix& Ab = Ab_.matrix();
Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
// We perform I += A'*A to the upper triangle
(*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
(*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2;
(*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
(*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
(*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
(*info)(slotB, slotB)(0, 0) += b.transpose() * b;
}
}
};
template<int M, int N1, int N2>
struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
BinaryJacobianFactor<M, N1, N2> > {
};
} //namespace gtsam

18
gtsam/linear/GaussianFactor.h
View File

@ -28,6 +28,8 @@ namespace gtsam {
// Forward declarations
class VectorValues;
class Scatter;
class SymmetricBlockMatrix;
/**
* An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a
@ -119,6 +121,14 @@ namespace gtsam {
*/
virtual GaussianFactor::shared_ptr negate() const = 0;
/** Update an information matrix by adding the information corresponding to this factor
* (used internally during elimination).
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
virtual void updateHessian(const FastVector<Key>& keys,
SymmetricBlockMatrix* info) const = 0;
/// y += alpha * A'*A*x
virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
@ -131,6 +141,12 @@ namespace gtsam {
/// Gradient wrt a key at any values
virtual Vector gradient(Key key, const VectorValues& x) const = 0;
// Determine position of a given key
template <typename CONTAINER>
static DenseIndex Slot(const CONTAINER& keys, Key key) {
return std::find(keys.begin(), keys.end(), key) - keys.begin();
}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -140,7 +156,7 @@ namespace gtsam {
}
}; // GaussianFactor
/// traits
template<>
struct traits<GaussianFactor> : public Testable<GaussianFactor> {

458
gtsam/linear/HessianFactor.cpp
View File

@ -15,17 +15,17 @@
* @date Dec 8, 2010
*/
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
@ -49,232 +49,185 @@
using namespace std;
using namespace boost::assign;
namespace br { using namespace boost::range; using namespace boost::adaptors; }
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
}
namespace gtsam {
/* ************************************************************************* */
string SlotEntry::toString() const {
ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
Scatter::Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering) {
gttic(Scatter_Constructor);
static const size_t none = std::numeric_limits<size_t>::max();
// First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
if (factor) {
for (GaussianFactor::const_iterator variable = factor->begin();
variable != factor->end(); ++variable) {
// TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
const JacobianFactor* asJacobian =
dynamic_cast<const JacobianFactor*>(factor.get());
if (!asJacobian || asJacobian->cols() > 1)
this->insert(
make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
}
}
}
// If we have an ordering, pre-fill the ordered variables first
size_t slot = 0;
if (ordering) {
BOOST_FOREACH(Key key, *ordering) {
const_iterator entry = find(key);
if (entry == end())
throw std::invalid_argument(
"The ordering provided to the HessianFactor Scatter constructor\n"
"contained extra variables that did not appear in the factors to combine.");
at(key).slot = (slot++);
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
BOOST_FOREACH(value_type& var_slot, *this) {
if (var_slot.second.slot == none)
var_slot.second.slot = (slot++);
}
}
/* ************************************************************************* */
HessianFactor::HessianFactor() :
info_(cref_list_of<1>(1))
{
info_(cref_list_of<1>(1)) {
linearTerm().setZero();
constantTerm() = 0.0;
}
/* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) :
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1))
{
if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0,0) = G;
info_(0,1) = g;
info_(1,1)(0,0) = f;
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) {
if (G.rows() != G.cols() || G.rows() != g.size())
throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0, 0) = G;
info_(0, 1) = g;
info_(1, 1)(0, 0) = f;
}
/* ************************************************************************* */
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
GaussianFactor(cref_list_of<1>(j)),
info_(cref_list_of<2> (Sigma.cols()) (1) )
{
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0,0) = Sigma.inverse(); // G
info_(0,1) = info_(0,0).selfadjointView() * mu; // g
info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) {
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0, 0) = Sigma.inverse(); // G
info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g
info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f
}
/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f) :
GaussianFactor(cref_list_of<2>(j1)(j2)),
info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) )
{
info_(0,0) = G11;
info_(0,1) = G12;
info_(0,2) = g1;
info_(1,1) = G22;
info_(1,2) = g2;
info_(2,2)(0,0) = f;
HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
double f) :
GaussianFactor(cref_list_of<2>(j1)(j2)), info_(
cref_list_of<3>(G11.cols())(G22.cols())(1)) {
info_(0, 0) = G11;
info_(0, 1) = G12;
info_(0, 2) = g1;
info_(1, 1) = G22;
info_(1, 2) = g2;
info_(2, 2)(0, 0) = f;
}
/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, Key j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f) :
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) )
{
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() ||
G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
info_(0,0) = G11;
info_(0,1) = G12;
info_(0,2) = G13;
info_(0,3) = g1;
info_(1,1) = G22;
info_(1,2) = G23;
info_(1,3) = g2;
info_(2,2) = G33;
info_(2,3) = g3;
info_(3,3)(0,0) = f;
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
double f) :
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_(
cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) {
if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
|| G11.rows() != G13.rows() || G11.rows() != g1.size()
|| G22.cols() != G12.cols() || G33.cols() != G13.cols()
|| G22.cols() != g2.size() || G33.cols() != g3.size())
throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
info_(0, 0) = G11;
info_(0, 1) = G12;
info_(0, 2) = G13;
info_(0, 3) = g1;
info_(1, 1) = G22;
info_(1, 2) = G23;
info_(1, 3) = g2;
info_(2, 2) = G33;
info_(2, 3) = g3;
info_(3, 3)(0, 0) = f;
}
/* ************************************************************************* */
namespace {
DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
DenseIndex _getSizeHF(const Vector& m) {
return m.size();
}
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
{
HessianFactor::HessianFactor(const std::vector<Key>& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
// Get the number of variables
size_t variable_count = js.size();
// Verify the provided number of entries in the vectors are consistent
if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2)
throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
if (gs.size() != variable_count
|| Gs.size() != (variable_count * (variable_count + 1)) / 2)
throw invalid_argument(
"Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
// Verify the dimensions of each provided matrix are consistent
// Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
for(size_t i = 0; i < variable_count; ++i){
for (size_t i = 0; i < variable_count; ++i) {
DenseIndex block_size = gs[i].size();
// Check rows
for(size_t j = 0; j < variable_count-i; ++j){
size_t index = i*(2*variable_count - i + 1)/2 + j;
if(Gs[index].rows() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
for (size_t j = 0; j < variable_count - i; ++j) {
size_t index = i * (2 * variable_count - i + 1) / 2 + j;
if (Gs[index].rows() != block_size) {
throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
// Check cols
for(size_t j = 0; j <= i; ++j){
size_t index = j*(2*variable_count - j + 1)/2 + (i-j);
if(Gs[index].cols() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
for (size_t j = 0; j <= i; ++j) {
size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j);
if (Gs[index].cols() != block_size) {
throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
}
// Fill in the blocks
size_t index = 0;
for(size_t i = 0; i < variable_count; ++i){
for(size_t j = i; j < variable_count; ++j){
for (size_t i = 0; i < variable_count; ++i) {
for (size_t j = i; j < variable_count; ++j) {
info_(i, j) = Gs[index++];
}
info_(i, variable_count) = gs[i];
}
info_(variable_count, variable_count)(0,0) = f;
info_(variable_count, variable_count)(0, 0) = f;
}
/* ************************************************************************* */
namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
{
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
gttic(HessianFactor_fromJacobian);
const SharedDiagonal& jfModel = jf.get_model();
if(jfModel)
{
if(jf.get_model()->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
info.full().triangularView() = jf.matrixObject().full().transpose() *
(jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() *
jf.matrixObject().full();
if (jfModel) {
if (jf.get_model()->isConstrained())
throw invalid_argument(
"Cannot construct HessianFactor from JacobianFactor with constrained noise model");
info.full().triangularView() =
jf.matrixObject().full().transpose()
* (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal()
* jf.matrixObject().full();
} else {
info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
info.full().triangularView() = jf.matrixObject().full().transpose()
* jf.matrixObject().full();
}
}
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const JacobianFactor& jf) :
GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject()))
{
GaussianFactor(jf), info_(
SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
_FromJacobianHelper(jf, info_);
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) :
GaussianFactor(gf)
{
GaussianFactor(gf) {
// Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf))
{
if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
_FromJacobianHelper(*jf, info_);
}
else if(const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf))
{
} else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
info_ = hf->info_;
}
else
{
throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
} else {
throw std::invalid_argument(
"In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
}
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
boost::optional<const Scatter&> scatter)
{
boost::optional<const Scatter&> scatter) {
gttic(HessianFactor_MergeConstructor);
boost::optional<Scatter> computedScatter;
if(!scatter) {
if (!scatter) {
computedScatter = Scatter(factors);
scatter = computedScatter;
}
@ -296,64 +249,57 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
// Form A' * A
gttic(update);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
if(factor) {
if(const HessianFactor* hessian = dynamic_cast<const HessianFactor*>(factor.get()))
updateATA(*hessian, *scatter);
else if(const JacobianFactor* jacobian = dynamic_cast<const JacobianFactor*>(factor.get()))
updateATA(*jacobian, *scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
if (factor)
factor->updateHessian(keys_, &info_);
gttoc(update);
}
/* ************************************************************************* */
void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const {
void HessianFactor::print(const std::string& s,
const KeyFormatter& formatter) const {
cout << s << "\n";
cout << " keys: ";
for(const_iterator key=this->begin(); key!=this->end(); ++key)
cout << formatter(*key) << "(" << this->getDim(key) << ") ";
for (const_iterator key = begin(); key != end(); ++key)
cout << formatter(*key) << "(" << getDim(key) << ") ";
cout << "\n";
gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: ");
gtsam::print(Matrix(info_.full().selfadjointView()),
"Augmented information matrix: ");
}
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if(!dynamic_cast<const HessianFactor*>(&lf))
if (!dynamic_cast<const HessianFactor*>(&lf))
return false;
else {
if(!Factor::equals(lf, tol))
if (!Factor::equals(lf, tol))
return false;
Matrix thisMatrix = this->info_.full().selfadjointView();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
Matrix thisMatrix = info_.full().selfadjointView();
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
Matrix rhsMatrix =
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
}
/* ************************************************************************* */
Matrix HessianFactor::augmentedInformation() const
{
Matrix HessianFactor::augmentedInformation() const {
return info_.full().selfadjointView();
}
/* ************************************************************************* */
Matrix HessianFactor::information() const
{
return info_.range(0, this->size(), 0, this->size()).selfadjointView();
Matrix HessianFactor::information() const {
return info_.range(0, size(), 0, size()).selfadjointView();
}
/* ************************************************************************* */
VectorValues HessianFactor::hessianDiagonal() const {
VectorValues d;
// Loop over all variables
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
Matrix B = info_(j, j).selfadjointView();
d.insert(keys_[j],B.diagonal());
d.insert(keys_[j], B.diagonal());
}
return d;
}
@ -366,26 +312,24 @@ void HessianFactor::hessianDiagonal(double* d) const {
}
/* ************************************************************************* */
map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
map<Key,Matrix> blocks;
map<Key, Matrix> HessianFactor::hessianBlockDiagonal() const {
map<Key, Matrix> blocks;
// Loop over all variables
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert it
Matrix B = info_(j, j).selfadjointView();
blocks.insert(make_pair(keys_[j],B));
blocks.insert(make_pair(keys_[j], B));
}
return blocks;
}
/* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const
{
Matrix HessianFactor::augmentedJacobian() const {
return JacobianFactor(*this).augmentedJacobian();
}
/* ************************************************************************* */
std::pair<Matrix, Vector> HessianFactor::jacobian() const
{
std::pair<Matrix, Vector> HessianFactor::jacobian() const {
return JacobianFactor(*this).jacobian();
}
@ -396,103 +340,34 @@ double HessianFactor::error(const VectorValues& c) const {
double xtg = 0, xGx = 0;
// extract the relevant subset of the VectorValues
// NOTE may not be as efficient
const Vector x = c.vector(this->keys());
const Vector x = c.vector(keys());
xtg = x.dot(linearTerm());
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x;
return 0.5 * (f - 2.0 * xtg + xGx);
xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x;
return 0.5 * (f - 2.0 * xtg + xGx);
}
/* ************************************************************************* */
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter)
{
gttic(updateATA);
// This function updates 'combined' with the information in 'update'. 'scatter' maps variables in
// the update factor to slots in the combined factor.
// First build an array of slots
gttic(slots);
FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) {
slots[slot] = scatter.at(j).slot;
++ slot;
}
gttoc(slots);
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
// Apply updates to the upper triangle
gttic(update);
size_t nrInfoBlocks = this->info_.nBlocks();
for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) {
DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2];
for(DenseIndex j1=0; j1<=j2; ++j1) {
DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1];
info_(slot1, slot2) += update.info_(j1, j2);
DenseIndex n = size(), N = info->nBlocks() - 1;
vector<DenseIndex> slots(n + 1);
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
for (DenseIndex i = 0; i <= j; ++i) {
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
(*info)(I, J) += info_(i, j);
}
}
gttoc(update);
}
/* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& update,
const Scatter& scatter) {
// This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined
// factor.
gttic(updateATA);
if (update.rows() > 0) {
gttic(whiten);
// Whiten the factor if it has a noise model
boost::optional<JacobianFactor> _whitenedFactor;
const JacobianFactor* whitenedFactor = &update;
if (update.get_model()) {
if (update.get_model()->isConstrained())
throw invalid_argument(
"Cannot update HessianFactor from JacobianFactor with constrained noise model");
_whitenedFactor = update.whiten();
whitenedFactor = &(*_whitenedFactor);
}
gttoc(whiten);
// A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below
const VerticalBlockMatrix& A = whitenedFactor->matrixObject();
// N is number of variables in HessianFactor, n in JacobianFactor
DenseIndex N = this->info_.nBlocks() - 1, n = A.nBlocks() - 1;
// First build an array of slots
gttic(slots);
FastVector<DenseIndex> slots(n + 1);
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update)
slots[slot++] = scatter.at(j).slot;
slots[n] = N;
gttoc(slots);
// Apply updates to the upper triangle
gttic(update);
// Loop over blocks of A, including RHS with j==n
for (DenseIndex j = 0; j <= n; ++j) {
DenseIndex J = slots[j]; // get block in Hessian
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
DenseIndex I = slots[i]; // get block in Hessian
info_(I, J).knownOffDiagonal() += A(i).transpose() * A(j);
}
// Fill diagonal block with Aj'*Aj
info_(J, J).selfadjointView().rankUpdate(A(j).transpose());
}
gttoc(update);
}
}
/* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const
{
GaussianFactor::shared_ptr HessianFactor::negate() const {
shared_ptr result = boost::make_shared<This>(*this);
result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
result->info_.full().triangularView() =
-result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
return result;
}
@ -509,7 +384,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// xj is the input vector
Vector xj = x.at(keys_[j]);
DenseIndex i = 0;
@ -518,13 +393,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView() * xj;
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex)size(); ++i)
for (i = j + 1; i < (DenseIndex) size(); ++i)
y[i] += info_(i, j).knownOffDiagonal() * xj;
}
// copy to yvalues
for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
bool didNotExist;
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
bool didNotExist;
VectorValues::iterator it;
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
if (didNotExist)
@ -539,7 +414,7 @@ VectorValues HessianFactor::gradientAtZero() const {
VectorValues g;
size_t n = size();
for (size_t j = 0; j < n; ++j)
g.insert(keys_[j], -info_(j,n).knownOffDiagonal());
g.insert(keys_[j], -info_(j, n).knownOffDiagonal());
return g;
}
@ -562,8 +437,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
if (i > j) {
Matrix Gji = info(j, i);
Gij = Gji.transpose();
}
else {
} else {
Gij = info(i, j);
}
// Accumulate Gij*xj to gradf
@ -575,30 +449,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
{
std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<HessianFactor> > EliminateCholesky(
const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminateCholesky);
// Build joint factor
HessianFactor::shared_ptr jointFactor;
try {
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys));
} catch(std::invalid_argument&) {
jointFactor = boost::make_shared<HessianFactor>(factors,
Scatter(factors, keys));
} catch (std::invalid_argument&) {
throw InvalidDenseElimination(
"EliminateCholesky was called with a request to eliminate variables that are not\n"
"involved in the provided factors.");
"involved in the provided factors.");
}
// Do dense elimination
GaussianConditional::shared_ptr conditional;
try {
size_t numberOfKeysToEliminate = keys.size();
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate);
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab);
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(
numberOfKeysToEliminate);
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(),
numberOfKeysToEliminate, Ab);
// Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
} catch(CholeskyFailed&) {
jointFactor->keys_.erase(jointFactor->begin(),
jointFactor->begin() + numberOfKeysToEliminate);
} catch (CholeskyFailed&) {
throw IndeterminantLinearSystemException(keys.front());
}
@ -607,9 +485,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
{
std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminatePreferCholesky);
// If any JacobianFactors have constrained noise models, we have to convert

41
gtsam/linear/HessianFactor.h
View File

@ -18,10 +18,10 @@
#pragma once
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/GaussianFactor.h>
#include <boost/make_shared.hpp>
@ -41,30 +41,6 @@ namespace gtsam {
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
/**
* One SlotEntry stores the slot index for a variable, as well its dimension.
*/
struct GTSAM_EXPORT SlotEntry {
size_t slot, dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
/**
* Scatter is an intermediate data structure used when building a HessianFactor
* incrementally, to get the keys in the right order. The "scatter" is a map from
* global variable indices to slot indices in the union of involved variables.
* We also include the dimensionality of the variable.
*/
class Scatter: public FastMap<Key, SlotEntry> {
public:
Scatter() {
}
Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering = boost::none);
};
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
@ -363,19 +339,12 @@ namespace gtsam {
/** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */
const SymmetricBlockMatrix& matrixObject() const { return info_; }
/** Update the factor by adding the information from the JacobianFactor
/** Update an information matrix by adding the information corresponding to this factor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;

39
gtsam/linear/JacobianFactor.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/VariableSlots.h>
@ -497,6 +497,43 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
return blocks;
}
/* ************************************************************************* */
void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_JacobianFactor);
if (rows() == 0) return;
// Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) {
if (model->isConstrained())
throw invalid_argument(
"JacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(infoKeys, info);
} else {
// Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
// Apply updates to the upper triangle
// Loop over blocks of A, including RHS with j==n
vector<DenseIndex> slots(n+1);
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
(*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
}
// Fill diagonal block with Aj'*Aj
(*info)(J, J).selfadjointView().rankUpdate(Ab_(j).transpose());
}
}
}
/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.rows());

7
gtsam/linear/JacobianFactor.h
View File

@ -273,6 +273,13 @@ namespace gtsam {
/** Get a view of the A matrix */
ABlock getA() { return Ab_.range(0, size()); }
/** Update an information matrix by adding the information corresponding to this factor
* (used internally during elimination).
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
/** Return A*x */
Vector operator*(const VectorValues& x) const;

7
gtsam/linear/NoiseModel.cpp
View File

@ -376,8 +376,11 @@ double Constrained::distance(const Vector& v) const {
/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
// selective scaling
return vector_scale(invsigmas(), H, true);
Matrix A = H;
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
if (!constrained(i)) // if constrained, leave row of A as is
A.row(i) *= invsigmas_(i);
return A;
}
/* ************************************************************************* */

21
gtsam/linear/NoiseModel.h
View File

@ -62,10 +62,11 @@ namespace gtsam {
Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {}
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual bool isConstrained() const {
return false; // default false
}
/// true if a constrained noise model, saves slow/clumsy dynamic casting
virtual bool isConstrained() const { return false; } // default false
/// true if a unit noise model, saves slow/clumsy dynamic casting
virtual bool isUnit() const { return false; } // default false
/// Dimensionality
inline size_t dim() const { return dim_;}
@ -80,6 +81,9 @@ namespace gtsam {
/// Whiten an error vector.
virtual Vector whiten(const Vector& v) const = 0;
/// Whiten a matrix.
virtual Matrix Whiten(const Matrix& H) const = 0;
/// Unwhiten an error vector.
virtual Vector unwhiten(const Vector& v) const = 0;
@ -390,9 +394,7 @@ namespace gtsam {
virtual ~Constrained() {}
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual bool isConstrained() const {
return true;
}
virtual bool isConstrained() const { return true; }
/// Return true if a particular dimension is free or constrained
bool constrained(size_t i) const;
@ -590,6 +592,9 @@ namespace gtsam {
return shared_ptr(new Unit(dim));
}
/// true if a unit noise model, saves slow/clumsy dynamic casting
virtual bool isUnit() const { return true; }
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
virtual Vector whiten(const Vector& v) const { return v; }
@ -854,6 +859,8 @@ namespace gtsam {
// TODO: functions below are dummy but necessary for the noiseModel::Base
inline virtual Vector whiten(const Vector& v) const
{ Vector r = v; this->WhitenSystem(r); return r; }
inline virtual Matrix Whiten(const Matrix& A) const
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline virtual Vector unwhiten(const Vector& /*v*/) const
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
inline virtual double distance(const Vector& v) const

81
gtsam/linear/Scatter.cpp Normal file
View File

@ -0,0 +1,81 @@
/* ----------------------------------------------------------------------------
* 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 Scatter.cpp
* @author Richard Roberts
* @author Frank Dellaert
* @date June 2015
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/inference/Ordering.h>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
string SlotEntry::toString() const {
ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
Scatter::Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering) {
gttic(Scatter_Constructor);
static const DenseIndex none = std::numeric_limits<size_t>::max();
// First do the set union.
BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) {
if (factor) {
for (GaussianFactor::const_iterator variable = factor->begin();
variable != factor->end(); ++variable) {
// TODO: Fix this hack to cope with zero-row Jacobians that come from
// BayesTreeOrphanWrappers
const JacobianFactor* asJacobian =
dynamic_cast<const JacobianFactor*>(factor.get());
if (!asJacobian || asJacobian->cols() > 1)
insert(
make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
}
}
}
// If we have an ordering, pre-fill the ordered variables first
size_t slot = 0;
if (ordering) {
BOOST_FOREACH (Key key, *ordering) {
const_iterator entry = find(key);
if (entry == end())
throw std::invalid_argument(
"The ordering provided to the HessianFactor Scatter constructor\n"
"contained extra variables that did not appear in the factors to "
"combine.");
at(key).slot = (slot++);
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
BOOST_FOREACH (value_type& var_slot, *this) {
if (var_slot.second.slot == none) var_slot.second.slot = (slot++);
}
}
/* ************************************************************************* */
} // gtsam

61
gtsam/linear/Scatter.h Normal file
View File

@ -0,0 +1,61 @@
/* ----------------------------------------------------------------------------
* 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 Scatter.h
* @brief Maps global variable indices to slot indices
* @author Richard Roberts
* @author Frank Dellaert
* @date June 2015
*/
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/dllexport.h>
#include <boost/optional.hpp>
namespace gtsam {
class GaussianFactorGraph;
class Ordering;
/// One SlotEntry stores the slot index for a variable, as well its dim.
struct GTSAM_EXPORT SlotEntry {
DenseIndex slot;
size_t dimension;
SlotEntry(DenseIndex _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
/**
* Scatter is an intermediate data structure used when building a HessianFactor
* incrementally, to get the keys in the right order. The "scatter" is a map
* from global variable indices to slot indices in the union of involved
* variables. We also include the dimensionality of the variable.
*/
class Scatter : public FastMap<Key, SlotEntry> {
public:
/// Constructor
Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering = boost::none);
/// Get the slot corresponding to the given key
DenseIndex slot(Key key) const { return at(key).slot; }
/// Get the dimension corresponding to the given key
DenseIndex dim(Key key) const { return at(key).dimension; }
};
} // \ namespace gtsam

20
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testCholeskyFactor.cpp
* @file testHessianFactor.cpp
* @author Richard Roberts
* @date Dec 15, 2010
*/
@ -38,6 +38,16 @@ using namespace gtsam;
const double tol = 1e-5;
/* ************************************************************************* */
TEST(HessianFactor, Slot)
{
FastVector<Key> keys = list_of(2)(4)(1);
EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist
}
/* ************************************************************************* */
TEST(HessianFactor, emptyConstructor)
{
@ -302,15 +312,17 @@ TEST(HessianFactor, CombineAndEliminate)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
Matrix93 A1; A1 << A11, A01, A21;
Vector9 b; b << b1, b0, b2;
Vector9 sigmas; sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// Make sure combining works
EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
// perform elimination on jacobian
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedRemainingFactor;

63
gtsam/linear/tests/testScatter.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 testScatter.cpp
* @author Frank Dellaert
* @date June, 2015
*/
#include <gtsam/linear/Scatter.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate) {
static const size_t m = 3, n = 3;
Matrix A01 =
(Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished();
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 =
(Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished();
Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0)
.finished();
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 =
(Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished();
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Scatter scatter(gfg);
EXPECT_LONGS_EQUAL(2, scatter.size());
EXPECT_LONGS_EQUAL(0, scatter.at(0).slot);
EXPECT_LONGS_EQUAL(1, scatter.at(1).slot);
EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension);
EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

74
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -54,65 +54,45 @@ public:
double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
bool diagonalDamping; ///< if true, use diagonal of Hessian
bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency)
bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (TODO: should be in state?)
bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor
double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
0.0), verbosityLM(SILENT), minModelFidelity(1e-3),
diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true),
min_diagonal_(1e-6), max_diagonal_(1e32) {
}
virtual ~LevenbergMarquardtParams() {
}
LevenbergMarquardtParams()
: lambdaInitial(1e-5),
lambdaFactor(10.0),
lambdaUpperBound(1e5),
lambdaLowerBound(0.0),
verbosityLM(SILENT),
minModelFidelity(1e-3),
diagonalDamping(false),
reuse_diagonal_(false),
useFixedLambdaFactor_(true),
min_diagonal_(1e-6),
max_diagonal_(1e32) {}
virtual ~LevenbergMarquardtParams() {}
virtual void print(const std::string& str = "") const;
inline double getlambdaInitial() const {
return lambdaInitial;
}
inline double getlambdaFactor() const {
return lambdaFactor;
}
inline double getlambdaUpperBound() const {
return lambdaUpperBound;
}
inline double getlambdaLowerBound() const {
return lambdaLowerBound;
}
inline double getlambdaInitial() const { return lambdaInitial; }
inline double getlambdaFactor() const { return lambdaFactor; }
inline double getlambdaUpperBound() const { return lambdaUpperBound; }
inline double getlambdaLowerBound() const { return lambdaLowerBound; }
inline std::string getVerbosityLM() const {
return verbosityLMTranslator(verbosityLM);
}
inline std::string getLogFile() const {
return logFile;
}
inline bool getDiagonalDamping() const {
return diagonalDamping;
}
inline std::string getLogFile() const { return logFile; }
inline bool getDiagonalDamping() const { return diagonalDamping; }
inline void setlambdaInitial(double value) {
lambdaInitial = value;
}
inline void setlambdaFactor(double value) {
lambdaFactor = value;
}
inline void setlambdaUpperBound(double value) {
lambdaUpperBound = value;
}
inline void setlambdaLowerBound(double value) {
lambdaLowerBound = value;
}
inline void setVerbosityLM(const std::string &s) {
inline void setlambdaInitial(double value) { lambdaInitial = value; }
inline void setlambdaFactor(double value) { lambdaFactor = value; }
inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
inline void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
inline void setVerbosityLM(const std::string& s) {
verbosityLM = verbosityLMTranslator(s);
}
inline void setLogFile(const std::string &s) {
logFile = s;
}
inline void setDiagonalDamping(bool flag) {
diagonalDamping = flag;
}
inline void setLogFile(const std::string& s) { logFile = s; }
inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
inline void setUseFixedLambdaFactor(bool flag) {
useFixedLambdaFactor_ = flag;
}

1
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -68,6 +68,7 @@ void NonlinearOptimizer::defaultOptimize() {
// Do next iteration
currentError = this->error();
this->iterate();
tictoc_finishedIteration();
// Maybe show output
if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues");

403
gtsam/slam/GeneralSFMFactor.h
View File

@ -25,13 +25,16 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/BinaryJacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/timing.h>
#include <boost/none.hpp>
#include <boost/optional/optional.hpp>
@ -48,209 +51,249 @@ class access;
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* The calibration is unknown here compared to GenericProjectionFactor
* @addtogroup SLAM
*/
template <class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* The calibration is unknown here compared to GenericProjectionFactor
* @addtogroup SLAM
*/
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
typedef Eigen::Matrix<double, 2, DimC> JacobianC;
typedef Eigen::Matrix<double, 2, DimL> JacobianL;
protected:
protected:
Point2 measured_; ///< the 2D measurement
Point2 measured_; ///< the 2D measurement
public:
public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark
*/
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
virtual ~GeneralSFMFactor() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
}
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, DimC);
if (H2) *H2 = zeros(2, DimL);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return zero(2);
}
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > {
};
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark
*/
template <class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value;
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
protected:
virtual ~GeneralSFMFactor() {} ///< destructor
Point2 measured_; ///< the 2D measurement
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
public:
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera; ///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; ///< typedef for the base class
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
}
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param poseKey is the index of the camera
* @param landmarkKey is the index of the landmark
* @param calibKey is the index of the calibration
*/
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
}
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero();
// TODO warn if verbose output asked for
return zero(2);
}
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
/// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1;
JacobianL H2;
Vector2 b;
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
b.setZero();
// TODO warn if verbose output asked for
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
// Whiten the system if needed
const SharedNoiseModel& noiseModel = this->get_noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include
// above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
};
template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
GeneralSFMFactor2<CALIBRATION> > {
};
// Create new (unit) noiseModel, preserving constraints if applicable
SharedDiagonal model;
if (noiseModel && noiseModel->isConstrained()) {
model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
}
return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > {
};
/**
* Non-linear factor for a constraint derived from a 2D measurement.
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
*/
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:
Point2 measured_; ///< the 2D measurement
public:
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param poseKey is the index of the camera
* @param landmarkKey is the index of the landmark
* @param calibKey is the index of the calibration
*/
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return zero(2);
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
GeneralSFMFactor2<CALIBRATION> > {
};
} //namespace

5
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -115,6 +115,11 @@ public:
return D;
}
virtual void updateHessian(const FastVector<Key>& keys,
SymmetricBlockMatrix* info) const {
throw std::runtime_error(
"RegularImplicitSchurFactor::updateHessian non implemented");
}
virtual Matrix augmentedJacobian() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::augmentedJacobian non implemented");

454
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -19,18 +19,20 @@
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/Testable.h>
#include <boost/assign/std/vector.hpp>
#include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost;
using namespace boost::assign;
#include <iostream>
#include <vector>
@ -49,7 +51,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
void addMeasurement(int i, int j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
@ -65,98 +68,99 @@ public:
};
static double getGaussian()
{
double S,V1,V2;
// Use Box-Muller method to create gauss noise from uniform noise
do
{
double U1 = rand() / (double)(RAND_MAX);
double U2 = rand() / (double)(RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2;
} while(S>=1);
return sqrt(-2.0f * (double)log(S) / S) * V1;
static double getGaussian() {
double S, V1, V2;
// Use Box-Muller method to create gauss noise from uniform noise
do {
double U1 = rand() / (double) (RAND_MAX);
double U2 = rand() / (double) (RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2;
} while (S >= 1);
return sqrt(-2.f * (double) log(S) / S) * V1;
}
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
static const double baseline = 5.;
/* ************************************************************************* */
TEST( GeneralSFMFactor, equals )
{
// Create two identical factors and make sure they're equal
Point2 z(323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
static vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> landmarks;
landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks;
}
boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
return X;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3_S2 K(640, 480, 0.1, 320, 240);
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
return X;
}
static boost::shared_ptr<Ordering> getOrdering(
const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for (size_t i = 0; i < landmarks.size(); ++i)
ordering->push_back(L(i));
for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, equals ) {
// Create two identical factors and make sure they're equal
Point2 z(323., 240.);
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> factor1(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor2(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2));
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
Point2 z(3., 0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(2));
Projection factor(z, sigma, X(1), L(1));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values)));
Point3 l1;
values.insert(L(1), l1);
EXPECT(
assert_equal(((Vector ) Vector2(-3., 0.)),
factor.unwhitenedError(values)));
}
static const double baseline = 5.0 ;
/* ************************************************************************* */
static vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> landmarks ;
landmarks.push_back(Point3 (-1.0,-1.0, z));
landmarks.push_back(Point3 (-1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0,-1.0, z));
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
return landmarks ;
}
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X ;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
return X ;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3_S2 K(640,480,0.01,320,240);
vector<GeneralCamera> X ;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
return X ;
}
static boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ;
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) {
@ -165,32 +169,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -202,38 +206,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
// add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
if ( i == 0 ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
}
else {
values.insert(L(i), landmarks[i]) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
if (i == 0) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
} else {
values.insert(L(i), landmarks[i]);
}
}
graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -246,35 +249,34 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1);
for (size_t i = 0; i < cameras.size(); ++i) {
for (size_t j = 0; j < landmarks.size(); ++j) {
Point2 z = cameras[i].project(landmarks[j]);
graph.addMeasurement(i, j, z, sigma1);
}
}
const size_t nMeasurements = landmarks.size()*cameras.size();
const size_t nMeasurements = landmarks.size() * cameras.size();
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(L(i), pt) ;
for (size_t j = 0; j < landmarks.size(); ++j) {
Point3 pt(landmarks[j].x() + noise * getGaussian(),
landmarks[j].y() + noise * getGaussian(),
landmarks[j].z() + noise * getGaussian());
values.insert(L(j), pt);
}
for ( size_t i = 0 ; i < cameras.size() ; ++i )
for (size_t i = 0; i < cameras.size(); ++i)
graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = landmarks.size()*cameras.size();
const size_t nMeasurements = landmarks.size() * cameras.size();
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
const double
rot_noise = 1e-5,
trans_noise = 1e-3,
focal_noise = 1,
skew_noise = 1e-5;
if ( i == 0 ) {
values.insert(X(i), cameras[i]) ;
}
else {
for (size_t i = 0; i < cameras.size(); ++i) {
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
skew_noise = 1e-5;
if (i == 0) {
values.insert(X(i), cameras[i]);
} else {
Vector delta = (Vector(11) <<
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y
skew_noise, // s
trans_noise, trans_noise // ux, uy
Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y
skew_noise, // s
trans_noise, trans_noise // ux, uy
).finished();
values.insert(X(i), cameras[i].retract(delta)) ;
values.insert(X(i), cameras[i].retract(delta));
}
}
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
values.insert(L(i), landmarks[i]) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
values.insert(L(i), landmarks[i]);
}
// fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i )
for (size_t i = 0; i < landmarks.size(); ++i)
graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
// add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
// Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.push_back(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(
RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init;
init.insert(X(0), GeneralCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
// The optimal value between the 2m range factor and 1m prior is 1.5m
Values expected;
expected.insert(X(0), GeneralCamera());
expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.)));
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(
PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.push_back(
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init;
init.insert(X(0), CalibratedCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
Values expected;
expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(X(0),
CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
LevenbergMarquardtParams params;
@ -431,5 +441,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
// Frank created these tests after switching to a custom LinearizedFactor
TEST(GeneralSFMFactor, BinaryJacobianFactor) {
Point2 measurement(3., -1.);
// Create Values
Values values;
Rot3 R;
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1;
values.insert(L(1), l1);
vector<SharedNoiseModel> models;
{
// Create various noise-models to test all cases
using namespace noiseModel;
Rot2 R = Rot2::fromAngle(0.3);
Matrix2 cov = R.matrix() * R.matrix().transpose();
models += SharedNoiseModel(), Unit::Create(2), //
Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov);
}
// Now loop over all these noise models
BOOST_FOREACH(SharedNoiseModel model, models) {
Projection factor(measurement, model, X(1), L(1));
// Test linearize
GaussianFactor::shared_ptr expected = //
factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
// Test methods that rely on updateHessian
if (model && !model->isConstrained()) {
// Construct HessianFactor from single JacobianFactor
HessianFactor expectedHessian(*expected), actualHessian(*actual);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9));
// Convert back
JacobianFactor actualJacobian(actualHessian);
// Note we do not expect the actualJacobian to match *expected
// Just that they have the same information on the variable.
EXPECT(
assert_equal(expected->augmentedInformation(),
actualJacobian.augmentedInformation(), 1e-9));
// Construct from GaussianFactorGraph
GaussianFactorGraph gfg1;
gfg1 += expected;
GaussianFactorGraph gfg2;
gfg2 += actual;
HessianFactor hessian1(gfg1), hessian2(gfg2);
EXPECT(assert_equal(hessian1, hessian2, 1e-9));
}
}
}
/* ************************************************************************* */
// Do a thorough test of BinaryJacobianFactor
TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) {
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> cameras = genCameraVariableCalibration();
Values values;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
for (size_t j = 0; j < landmarks.size(); ++j)
values.insert(L(j), landmarks[j]);
for (size_t i = 0; i < cameras.size(); ++i) {
for (size_t j = 0; j < landmarks.size(); ++j) {
Point2 z = cameras[i].project(landmarks[j]);
Projection::shared_ptr nonlinear = //
boost::make_shared<Projection>(z, sigma1, X(i), L(j));
GaussianFactor::shared_ptr factor = nonlinear->linearize(values);
HessianFactor hessian(*factor);
JacobianFactor jacobian(hessian);
EXPECT(
assert_equal(factor->augmentedInformation(),
jacobian.augmentedInformation(), 1e-9));
}
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

288
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
void addMeasurement(const int& i, const int& j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
@ -65,97 +66,101 @@ public:
};
static double getGaussian()
{
double S,V1,V2;
// Use Box-Muller method to create gauss noise from uniform noise
do
{
double U1 = rand() / (double)(RAND_MAX);
double U2 = rand() / (double)(RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2;
} while(S>=1);
return sqrt(-2.0f * (double)log(S) / S) * V1;
static double getGaussian() {
double S, V1, V2;
// Use Box-Muller method to create gauss noise from uniform noise
do {
double U1 = rand() / (double) (RAND_MAX);
double U2 = rand() / (double) (RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2;
} while (S >= 1);
return sqrt(-2.f * (double) log(S) / S) * V1;
}
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, equals )
{
TEST( GeneralSFMFactor_Cal3Bundler, equals ) {
// Create two identical factors and make sure they're equal
Point2 z(323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
Point2 z(323., 240.);
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor1(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor2(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2));
}
/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Point2 z(3.,0.);
Point2 z(3., 0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, X(1), L(1)));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values)));
Point3 l1;
values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;
static const double baseline = 5.;
/* ************************************************************************* */
static vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> landmarks ;
landmarks.push_back(Point3 (-1.0,-1.0, z));
landmarks.push_back(Point3 (-1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0,-1.0, z));
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
return landmarks ;
vector<Point3> landmarks;
landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks;
}
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> cameras ;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
return cameras ;
vector<GeneralCamera> cameras;
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
return cameras;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3Bundler K(500,1e-3,1e-3);
vector<GeneralCamera> cameras ;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
return cameras ;
const Cal3Bundler K(500, 1e-3, 1e-3);
vector<GeneralCamera> cameras;
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
return cameras;
}
static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
static boost::shared_ptr<Ordering> getOrdering(
const std::vector<GeneralCamera>& cameras,
const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ;
for (size_t i = 0; i < landmarks.size(); ++i)
ordering->push_back(L(i));
for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
}
/* ************************************************************************* */
@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
// add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
if ( i == 0 ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
}
else {
values.insert(L(i), landmarks[i]) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
if (i == 0) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
} else {
values.insert(L(i), landmarks[i]);
}
}
graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = landmarks.size()*cameras.size();
const size_t nMeasurements = landmarks.size() * cameras.size();
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(L(i), pt) ;
values.insert(L(i), pt);
}
for ( size_t i = 0 ; i < cameras.size() ; ++i )
for (size_t i = 0; i < cameras.size(); ++i)
graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = landmarks.size()*cameras.size();
const size_t nMeasurements = landmarks.size() * cameras.size();
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
const double
rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) {
values.insert(X(i), cameras[i]) ;
}
else {
for (size_t i = 0; i < cameras.size(); ++i) {
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
distort_noise = 1e-3;
if (i == 0) {
values.insert(X(i), cameras[i]);
} else {
Vector delta = (Vector(9) <<
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
).finished();
values.insert(X(i), cameras[i].retract(delta)) ;
values.insert(X(i), cameras[i].retract(delta));
}
}
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
values.insert(L(i), landmarks[i]) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
values.insert(L(i), landmarks[i]);
}
// fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i )
for (size_t i = 0; i < landmarks.size(); ++i)
graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
// add measurement with noise
Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ;
for (size_t j = 0; j < cameras.size(); ++j) {
for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1);
}
}
const size_t nMeasurements = cameras.size()*landmarks.size() ;
const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial
const double noise = baseline*0.1;
const double noise = baseline * 0.1;
Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(X(i), cameras[i]) ;
for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]);
// add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(landmarks[i].x()+noise*getGaussian(),
landmarks[i].y()+noise*getGaussian(),
landmarks[i].z()+noise*getGaussian());
values.insert(L(i), pt) ;
for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
// Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5 ;
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/symbolic/tests/testSymbolicJunctionTree.cpp
View File

@ -46,10 +46,10 @@ TEST( JunctionTree, constructor )
frontal1 = list_of(2)(3),
frontal2 = list_of(0)(1),
sep1, sep2 = list_of(2);
EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys));
EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys));
//EXPECT(assert_equal(sep1, actual.roots().front()->separator));
LONGS_EQUAL(1, (long)actual.roots().front()->factors.size());
EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys));
EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->orderedFrontalKeys));
//EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator));
LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size());
EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0]));

89
timing/timeSFMBAL.cpp Normal file
View File

@ -0,0 +1,89 @@
/* ----------------------------------------------------------------------------
* 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 timeSFMBAL.cpp
* @brief time structure from motion with BAL file
* @author Frank Dellaert
* @date June 6, 2015
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <stddef.h>
#include <stdexcept>
#include <string>
using namespace std;
using namespace gtsam;
//#define TERNARY
int main(int argc, char* argv[]) {
typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
using symbol_shorthand::P;
// Load BAL file (default is tiny)
string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data db;
bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
if (!success) throw runtime_error("Could not access file!");
// Build graph
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
}
Values initial = initialCamerasAndPointsEstimate(db);
// Create Schur-complement ordering
#ifdef CCOLAMD
vector<Key> pointKeys;
for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j));
Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true);
#else
Ordering ordering;
for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(i);
#endif
// Optimize
// Set parameters to be similar to ceres
LevenbergMarquardtParams params;
params.setOrdering(ordering);
params.setVerbosity("ERROR");
params.setVerbosityLM("TRYLAMBDA");
params.setDiagonalDamping(true);
params.setlambdaInitial(1e-4);
params.setlambdaFactor(2.0);
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values actual = lm.optimize();
tictoc_finishedIteration_();
tictoc_print_();
return 0;
}