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

Merge branch 'master' into new_wrap_local 

Conflicts:
	.cproject
	gtsam.h
release/4.3a0
Alex Cunningham 2012-06-30 20:04:12 +00:00
parent 133fc4ae5c
commit b33713a02e
49 changed files with 367 additions and 571 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -1484,6 +1484,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

2
gtsam.h
View File

@ -671,7 +671,7 @@ class VariableIndex {
size_t size() const;
size_t nFactors() const;
size_t nEntries() const;
void permute(const gtsam::Permutation& permutation);
void permuteInPlace(const gtsam::Permutation& permutation);
};*/
//*************************************************************************

7
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -49,10 +49,11 @@ namespace gtsam {
}
/* ************************************************************************* */
void DecisionTreeFactor::print(const string& s) const {
void DecisionTreeFactor::print(const string& s,
const IndexFormatter& formatter) const {
cout << s;
IndexFactor::print("IndexFactor:");
Potentials::print("Potentials:");
IndexFactor::print("IndexFactor:",formatter);
Potentials::print("Potentials:",formatter);
}
/* ************************************************************************* */

3
gtsam/discrete/DecisionTreeFactor.h
View File

@ -72,7 +72,8 @@ namespace gtsam {
bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const;
// print
void print(const std::string& s = "DecisionTreeFactor:\n") const;
virtual void print(const std::string& s = "DecisionTreeFactor:\n",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// @}
/// @name Standard Interface

5
gtsam/discrete/Potentials.cpp
View File

@ -51,10 +51,11 @@ namespace gtsam {
}
/* ************************************************************************* */
void Potentials::print(const string&s) const {
void Potentials::print(const string& s,
const IndexFormatter& formatter) const {
cout << s << "\n Cardinalities: ";
BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
cout << key.first << "=" << key.second << " ";
cout << formatter(key.first) << "=" << formatter(key.second) << " ";
cout << endl;
ADT::print(" ");
}

3
gtsam/discrete/Potentials.h
View File

@ -65,7 +65,8 @@ namespace gtsam {
// Testable
bool equals(const Potentials& other, double tol = 1e-9) const;
void print(const std::string& s = "Potentials: ") const;
void print(const std::string& s = "Potentials: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
size_t cardinality(Index j) const { return cardinalities_.at(j);}

49
gtsam/geometry/SimpleCamera.cpp Normal file
View File

@ -0,0 +1,49 @@
/* ----------------------------------------------------------------------------
* 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 SimpleCamera.cpp
* @brief A simple camera class with a Cal3_S2 calibration
* @date June 30, 2012
* @author Frank Dellaert
*/
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
namespace gtsam {
SimpleCamera simpleCamera(const Matrix& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}
}

5
gtsam/geometry/SimpleCamera.h
View File

@ -22,5 +22,10 @@
#include <gtsam/geometry/Cal3_S2.h>
namespace gtsam {
/// A simple camera class with a Cal3_S2 calibration
typedef PinholeCamera<Cal3_S2> SimpleCamera;
/// Recover camera from 3*4 camera matrix
SimpleCamera simpleCamera(const Matrix& P);
}

4
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -24,8 +24,8 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
Point2 p(1, -2);
static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
static Point2 p(1, -2);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)

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

@ -36,37 +36,20 @@ using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
BOOST_CLASS_EXPORT(gtsam::Point2)
BOOST_CLASS_EXPORT(gtsam::Point3)
BOOST_CLASS_EXPORT(gtsam::Pose2)
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Rot2)
BOOST_CLASS_EXPORT(gtsam::Rot3)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
static Point3 pt3(1.0, 2.0, 3.0);
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
static Pose3 pose3(rt3, pt3);
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
static Cal3Bundler cal3(1.0, 2.0, 3.0);
static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
static CalibratedCamera cal5(Pose3(rt3, pt3));
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
CalibratedCamera cal5(Pose3(rt3, pt3));
PinholeCamera<Cal3_S2> cam1(pose3, cal1);
StereoCamera cam2(pose3, cal4ptr);
StereoPoint2 spt(1.0, 2.0, 3.0);
static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
static StereoCamera cam2(pose3, cal4ptr);
static StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {

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

@ -133,6 +133,26 @@ TEST( SimpleCamera, Dproject_point_pose)
CHECK(assert_equal(Dpoint, numerical_point,1e-7));
}
/* ************************************************************************* */
TEST( SimpleCamera, simpleCamera)
{
Cal3_S2 K(468.2,427.2,91.2,300,200);
Rot3 R(
0.41380,0.90915,0.04708,
-0.57338,0.22011,0.78917,
0.70711,-0.35355,0.61237);
Point3 T(1000,2000,1500);
SimpleCamera expected(Pose3(R.inverse(),T),K);
// H&Z example, 2nd edition, page 163
Matrix P = Matrix_(3,4,
3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
-1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);
SimpleCamera actual = simpleCamera(P);
// Note precision of numbers given in book
CHECK(assert_equal(expected, actual,1e-1));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -74,21 +74,21 @@ TEST( StereoCamera, project)
/* ************************************************************************* */
Pose3 camera1(Matrix_(3,3,
static Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
static StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);
static Point3 p(0, 0, 5);
/* ************************************************************************* */
StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
TEST( StereoCamera, Dproject_stereo_pose)
{
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);

83
gtsam/inference/Permutation.h
View File

@ -26,7 +26,7 @@
namespace gtsam {
/**
/**
* A permutation reorders variables, for example to reduce fill-in during
* elimination. To save computation, the permutation can be applied to
* the necessary data structures only once, then multiple computations
@ -151,8 +151,8 @@ public:
*/
Permutation::shared_ptr inverse() const;
const_iterator begin() const { return rangeIndices_.begin(); } ///<TODO: comment
const_iterator end() const { return rangeIndices_.end(); } ///<TODO: comment
const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices
const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices
/// @}
@ -167,8 +167,8 @@ public:
*/
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
iterator begin() { return rangeIndices_.begin(); } ///<TODO: comment
iterator end() { return rangeIndices_.end(); } ///<TODO: comment
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices
protected:
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
@ -176,77 +176,4 @@ protected:
/// @}
};
/**
* Syntactic sugar for accessing another container through a permutation.
* Allows the syntax:
* Permuted<Container> permuted(permutation, container);
* permuted[index1];
* permuted[index2];
* which is equivalent to:
* container[permutation[index1]];
* container[permutation[index2]];
* but more concise.
*/
template<typename CONTAINER>
class Permuted {
Permutation permutation_;
CONTAINER& container_;
public:
typedef typename CONTAINER::iterator::value_type value_type;
/** Construct as a permuted view on the Container. The permutation is copied
* but only a reference to the container is stored.
*/
Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {}
/** Construct as a view on the Container with an identity permutation. Only
* a reference to the container is stored.
*/
Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
/** Print */
void print(const std::string& str = "") const {
std::cout << str;
permutation_.print(" permutation: ");
container_.print(" container: ");
}
/** Access the container through the permutation */
value_type& operator[](size_t index) { return container_[permutation_[index]]; }
/** Access the container through the permutation (const version) */
const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
/** Assignment operator for cloning in ISAM2 */
Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
permutation_ = other.permutation_;
container_ = other.container_;
return *this;
}
/** Permute this view by applying a permutation to the underlying permutation */
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
/** Access the underlying container */
CONTAINER* operator->() { return &container_; }
/** Access the underlying container (const version) */
const CONTAINER* operator->() const { return &container_; }
/** Size of the underlying container */
size_t size() const { return container_.size(); }
/** Access to the underlying container */
CONTAINER& container() { return container_; }
/** Access to the underlying container (const version) */
const CONTAINER& container() const { return container_; }
/** Access the underlying permutation */
Permutation& permutation() { return permutation_; }
const Permutation& permutation() const { return permutation_; }
};
}

4
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -85,8 +85,8 @@ namespace gtsam {
"IndexFactor::CombineAndEliminate called on factors with no variables.");
vector<Index> newKeys(keys.begin(), keys.end());
return make_pair(new IndexConditional(newKeys, nrFrontals),
new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end()));
return make_pair(boost::make_shared<IndexConditional>(newKeys, nrFrontals),
boost::make_shared<IndexFactor>(newKeys.begin() + nrFrontals, newKeys.end()));
}
/* ************************************************************************* */

52
gtsam/inference/VariableIndex.cpp
View File

@ -18,37 +18,12 @@
#include <iostream>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
VariableIndex::VariableIndex(const VariableIndex& other) :
index_(indexUnpermuted_) {
*this = other;
}
/* ************************************************************************* */
VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) {
index_ = rhs.index_;
nFactors_ = rhs.nFactors_;
nEntries_ = rhs.nEntries_;
return *this;
}
/* ************************************************************************* */
void VariableIndex::permute(const Permutation& permutation) {
#ifndef NDEBUG
// Assert that the permutation does not leave behind any non-empty variables,
// otherwise the nFactors and nEntries counts would be incorrect.
for(Index j=0; j<this->index_.size(); ++j)
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
assert(this->operator[](j).empty());
#endif
index_.permute(permutation);
}
/* ************************************************************************* */
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
@ -66,17 +41,13 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
/* ************************************************************************* */
void VariableIndex::print(const string& str) const {
cout << str << "\n";
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
Index var = 0;
BOOST_FOREACH(const Factors& variable, index_.container()) {
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
assert(rvar != index_.permutation().end());
cout << "var " << (rvar-index_.permutation().begin()) << ":";
BOOST_FOREACH(const size_t factor, variable)
for(Index var = 0; var < size(); ++var) {
cout << "var " << var << ":";
BOOST_FOREACH(const size_t factor, index_[var])
cout << " " << factor;
cout << "\n";
++ var;
}
cout << flush;
}
@ -85,7 +56,7 @@ void VariableIndex::print(const string& str) const {
void VariableIndex::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges.
BOOST_FOREACH(const Factors& variable, index_.container()) {
BOOST_FOREACH(const Factors& variable, index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, variable)
os << (factor+1) << " "; // base 1
@ -94,4 +65,15 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
os << flush;
}
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order
vector<VariableIndex::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]);
// Move reference to entire index into the VariableIndex
index_.swap(newIndex);
}
}

50
gtsam/inference/VariableIndex.h
View File

@ -22,10 +22,12 @@
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/base/types.h>
namespace gtsam {
class Permutation;
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of
@ -44,8 +46,7 @@ public:
typedef Factors::const_iterator Factor_const_iterator;
protected:
std::vector<Factors> indexUnpermuted_;
Permuted<std::vector<Factors> > index_; // Permuted view of indexUnpermuted.
std::vector<Factors> index_;
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
@ -55,7 +56,7 @@ public:
/// @{
/** Default constructor, creates an empty VariableIndex */
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
VariableIndex() : nFactors_(0), nEntries_(0) {}
/**
* Create a VariableIndex that computes and stores the block column structure
@ -70,16 +71,6 @@ public:
*/
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
/**
* Copy constructor
*/
VariableIndex(const VariableIndex& other);
/**
* Assignment operator
*/
VariableIndex& operator=(const VariableIndex& rhs);
/// @}
/// @name Standard Interface
/// @{
@ -120,9 +111,6 @@ public:
/// @name Advanced Interface
/// @{
/** Access a list of factors by variable */
Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally.
@ -137,11 +125,8 @@ public:
*/
template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors);
/**
* Apply a variable permutation. Does not rearrange data, just permutes
* future lookups by variable.
*/
void permute(const Permutation& permutation);
/// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& permutation);
protected:
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
@ -150,13 +135,13 @@ protected:
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } ///<TODO: comment
///TODO: comment
VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
/// Internal constructor to allocate a VariableIndex of the requested size
VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
///TODO: comment
/// Internal check of the validity of a variable
void checkVar(Index variable) const { assert(variable < index_.size()); }
///TODO: comment
/// Internal function to populate the variable index from a factor graph
template<class FactorGraph> void fill(const FactorGraph& factorGraph);
/// @}
@ -183,7 +168,7 @@ void VariableIndex::fill(const FactorGraph& factorGraph) {
/* ************************************************************************* */
template<class FactorGraph>
VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
nFactors_(0), nEntries_(0) {
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
@ -200,8 +185,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
}
// Allocate array
index_.container().resize(maxVar+1);
index_.permutation() = Permutation::Identity(maxVar+1);
index_.resize(maxVar+1);
fill(factorGraph);
}
@ -210,7 +194,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
/* ************************************************************************* */
template<class FactorGraph>
VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
index_(nVariables), nFactors_(0), nEntries_(0) {
fill(factorGraph);
}
@ -232,11 +216,7 @@ void VariableIndex::augment(const FactorGraph& factors) {
}
// Allocate index
Index originalSize = index_.size();
index_.container().resize(std::max(index_.size(), maxVar+1));
index_.permutation().resize(index_.container().size());
for(Index var=originalSize; var<index_.permutation().size(); ++var)
index_.permutation()[var] = var;
index_.resize(std::max(index_.size(), maxVar+1));
// Augment index mapping from variable id to factor index
size_t orignFactors = nFactors_;

2
gtsam/inference/tests/testISAM.cpp
View File

@ -34,7 +34,7 @@ typedef ISAM<IndexConditional> SymbolicISAM;
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
/* ************************************************************************* */

36
gtsam/linear/GaussianConditional.cpp
View File

@ -182,34 +182,18 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
return JacobianFactor::shared_ptr(new JacobianFactor(*this));
}
/* ************************************************************************* */
template<class VALUES>
inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) {
// Helper function to solve-in-place on a VectorValues or Permuted<VectorValues>,
// called by GaussianConditional::solveInPlace(VectorValues&) and by
// GaussianConditional::solveInPlace(Permuted<VectorValues>&).
static const bool debug = false;
if(debug) conditional.print("Solving conditional in place");
Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
xS = conditional.get_d() - conditional.get_S() * xS;
Vector soln = conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
if(debug) {
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: ");
}
internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
}
/* ************************************************************************* */
void GaussianConditional::solveInPlace(VectorValues& x) const {
doSolveInPlace(*this, x); // Call helper version above
}
/* ************************************************************************* */
void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
doSolveInPlace(*this, x); // Call helper version above
static const bool debug = false;
if(debug) this->print("Solving conditional in place");
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
xS = this->get_d() - this->get_S() * xS;
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
if(debug) {
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: ");
}
internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
}
/* ************************************************************************* */

17
gtsam/linear/GaussianConditional.h
View File

@ -196,23 +196,6 @@ public:
*/
void solveInPlace(VectorValues& x) const;
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional (version for permuted
* VectorValues). The parents are assumed to have already been solved in
* and their values are read from \c x. This function works for multiple
* frontal variables.
*
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
* variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
*
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
* solution \f$ x_f \f$ will be written.
*/
void solveInPlace(Permuted<VectorValues>& x) const;
// functions for transpose backsubstitution
/**

2
gtsam/linear/JacobianFactor.h
View File

@ -84,7 +84,7 @@ namespace gtsam {
protected:
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
std::vector<size_t> firstNonzeroBlocks_;
AbMatrix matrix_; // the full matrix corresponding to the factor
BlockAb Ab_; // the block view of the full matrix

2
gtsam/linear/NoiseModel.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <cmath>

40
gtsam/linear/VectorValues.cpp
View File

@ -17,10 +17,12 @@
*/
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/linear/VectorValues.h>
using namespace std;
using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
VectorValues::VectorValues(const VectorValues& other) {
@ -166,20 +168,24 @@ void VectorValues::operator+=(const VectorValues& c) {
}
/* ************************************************************************* */
VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
if(this->size() != rhs.size())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
for(size_t j=0; j<this->size(); ++j) {
if(exists(j)) {
SubVector& l(this->at(j));
const SubVector& r(rhs[j]);
if(l.rows() != r.rows())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
l = r;
} else {
if(rhs.container().exists(rhs.permutation()[j]))
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
}
}
return *this;
VectorValues VectorValues::permute(const Permutation& permutation) const {
// Create result and allocate space
VectorValues lhs;
lhs.values_.resize(this->dim());
lhs.maps_.reserve(this->size());
// Copy values from this VectorValues to the permuted VectorValues
size_t lhsPos = 0;
for(size_t i = 0; i < this->size(); ++i) {
// Map the next LHS subvector to the next slice of the LHS vector
lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
// Copy the data from the RHS subvector to the LHS subvector
lhs.maps_[i] = this->at(permutation[i]);
// Increment lhs position
lhsPos += lhs.maps_[i].size();
}
return lhs;
}
}

13
gtsam/linear/VectorValues.h
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Permutation.h>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
@ -29,6 +28,9 @@
namespace gtsam {
// Forward declarations
class Permutation;
/**
* This class represents a collection of vector-valued variables associated
* each with a unique integer index. It is typically used to store the variables
@ -288,10 +290,11 @@ namespace gtsam {
*/
void operator+=(const VectorValues& c);
/** Assignment operator from Permuted<VectorValues>, requires the dimensions
* of the assignee to already be properly pre-allocated.
*/
VectorValues& operator=(const Permuted<VectorValues>& rhs);
/**
* Permute the entries of this VectorValues, returns a new VectorValues as
* the result.
*/
VectorValues permute(const Permutation& permutation) const;
/// @}

49
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -267,55 +267,6 @@ TEST( GaussianConditional, solve_multifrontal )
}
/* ************************************************************************* */
TEST( GaussianConditional, solve_multifrontal_permuted )
{
// create full system, 3 variables, 2 frontals, all 2 dim
Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
// 3 variables, all dim=2
vector<size_t> dims; dims += 2, 2, 2, 1;
GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
Vector sigmas = ones(4);
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
// partial solution
Vector sl1 = Vector_(2, 9.0, 10.0);
// elimination order; _x_, _x1_, _l1_
VectorValues actualUnpermuted(vector<size_t>(3, 2));
Permutation permutation(3);
permutation[0] = 2;
permutation[1] = 0;
permutation[2] = 1;
Permuted<VectorValues> actual(permutation, actualUnpermuted);
actual[_x_] = Vector_(2, 0.1, 0.2); // rhs
actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
actual[_l1_] = sl1; // parent
VectorValues expectedUnpermuted(vector<size_t>(3, 2));
Permuted<VectorValues> expected(permutation, expectedUnpermuted);
expected[_x_] = Vector_(2, -3.1,-3.4);
expected[_x1_] = Vector_(2, -11.9,-13.2);
expected[_l1_] = sl1;
// verify indices/size
EXPECT_LONGS_EQUAL(3, cg.size());
EXPECT_LONGS_EQUAL(4, cg.dim());
// solve and verify
cg.solveInPlace(actual);
EXPECT(assert_equal(expected.container(), actual.container(), tol));
}
/* ************************************************************************* */
TEST( GaussianConditional, solveTranspose ) {
static const Index _y_=1;

2
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -35,7 +35,7 @@ using namespace gtsam;
static const Index x2=0, x1=1, x3=2, x4=3;
GaussianFactorGraph createChain() {
static GaussianFactorGraph createChain() {
typedef GaussianFactorGraph::sharedFactor Factor;
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);

10
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -47,11 +47,11 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST (Serialization, noiseModels) {

59
gtsam/linear/tests/testVectorValues.cpp
View File

@ -421,52 +421,31 @@ TEST(VectorValues, hasSameStructure) {
EXPECT(!v1.hasSameStructure(VectorValues()));
}
/* ************************************************************************* */
TEST(VectorValues, permuted_combined) {
Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
TEST(VectorValues, permute) {
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
VectorValues combined(dims);
combined[0] = v1;
combined[1] = v2;
combined[2] = v3;
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(2, Vector_(2, 4.0, 5.0));
original.insert(3, Vector_(2, 6.0, 7.0));
Permutation perm1(3);
perm1[0] = 1;
perm1[1] = 2;
perm1[2] = 0;
VectorValues expected;
expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2
expected.insert(1, Vector_(1, 1.0)); // from 0
expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3
expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1
Permutation perm2(3);
perm2[0] = 1;
perm2[1] = 2;
perm2[2] = 0;
Permutation permutation(4);
permutation[0] = 2;
permutation[1] = 0;
permutation[2] = 3;
permutation[3] = 1;
Permuted<VectorValues> permuted1(combined);
CHECK(assert_equal(v1, permuted1[0]))
CHECK(assert_equal(v2, permuted1[1]))
CHECK(assert_equal(v3, permuted1[2]))
VectorValues actual = original.permute(permutation);
permuted1.permute(perm1);
CHECK(assert_equal(v1, permuted1[2]))
CHECK(assert_equal(v2, permuted1[0]))
CHECK(assert_equal(v3, permuted1[1]))
permuted1.permute(perm2);
CHECK(assert_equal(v1, permuted1[1]))
CHECK(assert_equal(v2, permuted1[2]))
CHECK(assert_equal(v3, permuted1[0]))
Permuted<VectorValues> permuted2(perm1, combined);
CHECK(assert_equal(v1, permuted2[2]))
CHECK(assert_equal(v2, permuted2[0]))
CHECK(assert_equal(v3, permuted2[1]))
permuted2.permute(perm2);
CHECK(assert_equal(v1, permuted2[1]))
CHECK(assert_equal(v2, permuted2[2]))
CHECK(assert_equal(v3, permuted2[0]))
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */

67
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -29,8 +29,8 @@ namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector<bool>& replacedKeys,
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
@ -40,28 +40,21 @@ void ISAM2::Impl::AddVariables(
std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
const size_t originalDim = delta->dim();
const size_t originalnVars = delta->size();
delta.container().append(dims);
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
deltaNewton.container().append(dims);
deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaNewton.permutation().resize(originalnVars + newTheta.size());
deltaGradSearch.container().append(dims);
deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaGradSearch.permutation().resize(originalnVars + newTheta.size());
const size_t originalDim = delta.dim();
const size_t originalnVars = delta.size();
delta.append(dims);
delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaNewton.append(dims);
deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaGradSearch.append(dims);
deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
{
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
deltaNewton.permutation()[nextVar] = nextVar;
deltaGradSearch.permutation()[nextVar] = nextVar;
ordering.insert(key_value.key, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
}
@ -82,7 +75,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -110,7 +103,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const Permuted<VectorValues>& delta, const ISAM2Clique::shared_ptr& clique) {
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) {
// Check the current clique for relinearization
bool relinearize = false;
@ -131,7 +124,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const Permuted<VectorValues>& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
// Check the current clique for relinearization
bool relinearize = false;
@ -163,7 +156,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -201,13 +194,13 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys,
}
/* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
#ifdef NDEBUG
invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
invalidateIfDebug = boost::none;
#endif
assert(values.size() == ordering.nVars());
@ -304,7 +297,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
toc(4,"ccolamd permutations");
tic(5,"permute affected variable index");
affectedFactorsIndex.permute(*affectedColamd);
affectedFactorsIndex.permuteInPlace(*affectedColamd);
toc(5,"permute affected variable index");
tic(6,"permute affected factors");
@ -354,25 +347,13 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
}
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;
if (wildfireThreshold <= 0.0) {
// Threshold is zero or less, so do a full recalculation
// Collect dimensions and allocate new VectorValues
vector<size_t> dims(delta.size());
for(size_t j=0; j<delta.size(); ++j)
dims[j] = delta->dim(j);
VectorValues newDelta(dims);
// Optimize full solution delta
internal::optimizeInPlace(root, newDelta);
// Copy solution into delta
delta.permutation() = Permutation::Identity(delta.size());
delta.container() = newDelta;
internal::optimizeInPlace(root, delta);
lastBacksubVariableCount = delta.size();
} else {
@ -380,8 +361,8 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
#ifndef NDEBUG
for(size_t j=0; j<delta.container().size(); ++j)
assert(delta.container()[j].unaryExpr(ptr_fun(isfinite<double>)).all());
for(size_t j=0; j<delta.size(); ++j)
assert(delta[j].unaryExpr(ptr_fun(isfinite<double>)).all());
#endif
}
@ -394,7 +375,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
/* ************************************************************************* */
namespace internal {
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need
// update deltas and recurse to children, but if not, we do not need to
@ -433,7 +414,7 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vecto
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
VectorValues& deltaNewton, VectorValues& RgProd) {
// Get gradient
VectorValues grad = *allocateVectorValues(isam);

16
gtsam/nonlinear/ISAM2-impl.h
View File

@ -46,8 +46,8 @@ struct ISAM2::Impl {
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, std::vector<bool>& replacedKeys,
static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys,
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -68,7 +68,7 @@ struct ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
static FastSet<Index> CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -82,7 +82,7 @@ struct ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -115,9 +115,9 @@ struct ISAM2::Impl {
* recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
static void ExpmapMasked(Values& values, const VectorValues& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -137,10 +137,10 @@ struct ISAM2::Impl {
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
const ReorderingMode& reorderingMode, bool useQR);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold);
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
VectorValues& deltaNewton, VectorValues& RgProd);
};

4
gtsam/nonlinear/ISAM2-inl.h
View File

@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
std::vector<bool>& changed, const std::vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
@ -114,7 +114,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
/* ************************************************************************* */
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, Permuted<VectorValues>& delta) {
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
std::vector<bool> changed(keys.size(), false);
int count = 0;
// starting from the root, call optimize on each conditional

30
gtsam/nonlinear/ISAM2.cpp
View File

@ -41,7 +41,6 @@ static const double batchThreshold = 0.65;
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params):
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@ -49,15 +48,13 @@ ISAM2::ISAM2(const ISAM2Params& params):
/* ************************************************************************* */
ISAM2::ISAM2():
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true) {
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2& other):
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) {
ISAM2::ISAM2(const ISAM2& other) {
*this = other;
}
@ -308,12 +305,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// Reorder
tic(2,"permute global variable index");
variableIndex_.permute(*colamd);
variableIndex_.permuteInPlace(*colamd);
toc(2,"permute global variable index");
tic(3,"permute delta");
delta_.permute(*colamd);
deltaNewton_.permute(*colamd);
RgProd_.permute(*colamd);
delta_ = delta_.permute(*colamd);
deltaNewton_ = deltaNewton_.permute(*colamd);
RgProd_ = RgProd_.permute(*colamd);
toc(3,"permute delta");
tic(4,"permute ordering");
ordering_.permuteWithInverse(*colamdInverse);
@ -429,12 +426,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// re-eliminate. The reordered variables are also mentioned in the
// orphans and the leftover cached factors.
tic(3,"permute global variable index");
variableIndex_.permute(partialSolveResult.fullReordering);
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
toc(3,"permute global variable index");
tic(4,"permute delta");
delta_.permute(partialSolveResult.fullReordering);
deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_.permute(partialSolveResult.fullReordering);
delta_ = delta_.permute(partialSolveResult.fullReordering);
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
toc(4,"permute delta");
tic(5,"permute ordering");
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@ -723,8 +720,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
tic(2, "Copy dx_d");
// Update Delta and linear step
doglegDelta_ = doglegResult.Delta;
delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation
delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
toc(2, "Copy dx_d");
}
@ -739,7 +735,7 @@ Values ISAM2::calculateEstimate() const {
Values ret(theta_);
toc(1, "Copy Values");
tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta());
const VectorValues& delta(getDelta());
toc(2, "getDelta");
tic(3, "Expmap");
vector<bool> mask(ordering_.nVars(), true);
@ -756,7 +752,7 @@ Values ISAM2::calculateBestEstimate() const {
}
/* ************************************************************************* */
const Permuted<VectorValues>& ISAM2::getDelta() const {
const VectorValues& ISAM2::getDelta() const {
if(!deltaUptodate_)
updateDelta();
return delta_;
@ -829,7 +825,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
tic(3, "Compute minimizing step size");
// Compute minimizing step size
double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
double RgNormSq = isam.RgProd_.vector().squaredNorm();
double step = -gradientSqNorm / RgNormSq;
toc(3, "Compute minimizing step size");

34
gtsam/nonlinear/ISAM2.h
View File

@ -206,7 +206,7 @@ struct ISAM2Result {
* factors passed as \c newFactors to ISAM2::update(). These indices may be
* used later to refer to the factors in order to remove them.
*/
FastVector<size_t> newFactorsIndices;
std::vector<size_t> newFactorsIndices;
/** A struct holding detailed results, which must be enabled with
* ISAM2Params::enableDetailedResults.
@ -347,26 +347,16 @@ protected:
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
VariableIndex variableIndex_;
/** The linear delta from the last linear solution, an update to the estimate in theta */
VectorValues deltaUnpermuted_;
/** The linear delta from the last linear solution, an update to the estimate in theta
*
* This is \c mutable because it is a "cached" variable - it is not updated
* until either requested with getDelta() or calculateEstimate(), or needed
* during update() to evaluate whether to relinearize variables.
*/
mutable VectorValues delta_;
/** The permutation through which the deltaUnpermuted_ is
* referenced.
*
* Permuting Vector entries would be slow, so for performance we
* instead maintain this permutation through which we access the linear delta
* indirectly
*
* This is \c mutable because it is a "cached" variable - it is not updated
* until either requested with getDelta() or calculateEstimate(), or needed
* during update() to evaluate whether to relinearize variables.
*/
mutable Permuted<VectorValues> delta_;
VectorValues deltaNewtonUnpermuted_;
mutable Permuted<VectorValues> deltaNewton_;
VectorValues RgProdUnpermuted_;
mutable Permuted<VectorValues> RgProd_;
mutable VectorValues deltaNewton_;
mutable VectorValues RgProd_;
mutable bool deltaDoglegUptodate_;
/** Indicates whether the current delta is up-to-date, only used
@ -497,7 +487,7 @@ public:
Values calculateBestEstimate() const;
/** Access the current delta, computed during the last call to update */
const Permuted<VectorValues>& getDelta() const;
const VectorValues& getDelta() const;
/** Access the set of nonlinear factors */
const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
@ -555,7 +545,7 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
/// @return The number of variables that were solved for
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
double threshold, const std::vector<bool>& replaced, VectorValues& delta);
/**
* Optimize along the gradient direction, with a closed-form computation to

2
gtsam/nonlinear/Values.cpp
View File

@ -120,7 +120,7 @@ namespace gtsam {
/* ************************************************************************* */
void Values::insert(Key j, const Value& val) {
Key key = j; // Non-const duplicate to deal with non-const insert argument
std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
std::pair<KeyValueMap::iterator,bool> insertResult = values_.insert(key, val.clone_());
if(!insertResult.second)
throw ValuesKeyAlreadyExists(j);
}

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

@ -47,13 +47,13 @@ typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
static Point3 pt3(1.0, 2.0, 3.0);
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
static Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
static Cal3Bundler cal3(1.0, 2.0, 3.0);
TEST (Serialization, TemplatedValues) {
Values values;

8
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -28,18 +28,18 @@
using namespace std;
using namespace gtsam;
Pose3 camera1(Matrix_(3,3,
static Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
static StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);
static Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// Convenience for named keys

5
gtsam_unstable/discrete/AllDiff.cpp
View File

@ -20,10 +20,11 @@ namespace gtsam {
}
/* ************************************************************************* */
void AllDiff::print(const std::string& s) const {
void AllDiff::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << "AllDiff on ";
BOOST_FOREACH (Index dkey, keys_)
std::cout << dkey << " ";
std::cout << formatter(dkey) << " ";
std::cout << std::endl;
}

3
gtsam_unstable/discrete/AllDiff.h
View File

@ -34,7 +34,8 @@ namespace gtsam {
AllDiff(const DiscreteKeys& dkeys);
// print
virtual void print(const std::string& s = "") const;
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// Calculate value = expensive !
virtual double operator()(const Values& values) const;

7
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -33,9 +33,10 @@ namespace gtsam {
}
// print
virtual void print(const std::string& s = "") const {
std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1]
<< std::endl;
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const {
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
<< formatter(keys_[1]) << std::endl;
}
/// Calculate value

7
gtsam_unstable/discrete/Domain.cpp
View File

@ -15,9 +15,10 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
void Domain::print(const string& s) const {
// cout << s << ": Domain on " << keys_[0] << " (j=" << keys_[0]
// << ") with values";
void Domain::print(const string& s,
const IndexFormatter& formatter) const {
// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" <<
// formatter(keys_[0]) << ") with values";
// BOOST_FOREACH (size_t v,values_) cout << " " << v;
// cout << endl;
BOOST_FOREACH (size_t v,values_) cout << v;

3
gtsam_unstable/discrete/Domain.h
View File

@ -66,7 +66,8 @@ namespace gtsam {
}
// print
virtual void print(const std::string& s = "") const;
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
bool contains(size_t value) const {
return values_.count(value)>0;

5
gtsam_unstable/discrete/SingleValue.cpp
View File

@ -16,8 +16,9 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
void SingleValue::print(const string& s) const {
cout << s << "SingleValue on " << "j=" << keys_[0]
void SingleValue::print(const string& s,
const IndexFormatter& formatter) const {
cout << s << "SingleValue on " << "j=" << formatter(keys_[0])
<< " with value " << value_ << endl;
}

3
gtsam_unstable/discrete/SingleValue.h
View File

@ -42,7 +42,8 @@ namespace gtsam {
}
// print
virtual void print(const std::string& s = "") const;
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// Calculate value
virtual double operator()(const Values& values) const;

2
gtsam_unstable/discrete/tests/testScheduler.cpp
View File

@ -149,7 +149,7 @@ TEST( schedulingExample, test)
/* ************************************************************************* */
TEST( schedulingExample, smallFromFile)
{
string path("../../../gtsam_unstable/discrete/examples/");
string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/");
Scheduler s(2, path + "small.csv");
// add areas

4
tests/testGaussianISAM.cpp
View File

@ -41,10 +41,10 @@ using symbol_shorthand::L;
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests
double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
const double tol = 1e-4;
static const double tol = 1e-4;
/* ************************************************************************* */
TEST_UNSAFE( ISAM, iSAM_smoother )

111
tests/testGaussianISAM2.cpp
View File

@ -139,112 +139,69 @@ TEST_UNSAFE(ISAM2, AddVariables) {
// Create initial state
Values theta;
theta.insert((0), Pose2(.1, .2, .3));
theta.insert(0, Pose2(.1, .2, .3));
theta.insert(100, Point2(.4, .5));
Values newTheta;
newTheta.insert((1), Pose2(.6, .7, .8));
newTheta.insert(1, Pose2(.6, .7, .8));
VectorValues deltaUnpermuted;
deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaUnpermuted.insert(1, Vector_(2, .4, .5));
VectorValues delta;
delta.insert(0, Vector_(3, .1, .2, .3));
delta.insert(1, Vector_(2, .4, .5));
Permutation permutation(2);
permutation[0] = 1;
permutation[1] = 0;
VectorValues deltaNewton;
deltaNewton.insert(0, Vector_(3, .1, .2, .3));
deltaNewton.insert(1, Vector_(2, .4, .5));
Permuted<VectorValues> delta(permutation, deltaUnpermuted);
VectorValues deltaNewtonUnpermuted;
deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationNewton(2);
permutationNewton[0] = 1;
permutationNewton[1] = 0;
Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
VectorValues deltaRgUnpermuted;
deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationRg(2);
permutationRg[0] = 1;
permutationRg[1] = 0;
Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
VectorValues deltaRg;
deltaRg.insert(0, Vector_(3, .1, .2, .3));
deltaRg.insert(1, Vector_(2, .4, .5));
vector<bool> replacedKeys(2, false);
Ordering ordering; ordering += 100, (0);
Ordering ordering; ordering += 100, 0;
ISAM2::Nodes nodes(2);
// Verify initial state
LONGS_EQUAL(0, ordering[100]);
LONGS_EQUAL(1, ordering[(0)]);
EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]]));
EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]]));
LONGS_EQUAL(1, ordering[0]);
EXPECT(assert_equal(delta[0], delta[ordering[100]]));
EXPECT(assert_equal(delta[1], delta[ordering[0]]));
// Create expected state
Values thetaExpected;
thetaExpected.insert((0), Pose2(.1, .2, .3));
thetaExpected.insert(0, Pose2(.1, .2, .3));
thetaExpected.insert(100, Point2(.4, .5));
thetaExpected.insert((1), Pose2(.6, .7, .8));
thetaExpected.insert(1, Pose2(.6, .7, .8));
VectorValues deltaUnpermutedExpected;
deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
VectorValues deltaExpected;
deltaExpected.insert(0, Vector_(3, .1, .2, .3));
deltaExpected.insert(1, Vector_(2, .4, .5));
deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationExpected(3);
permutationExpected[0] = 1;
permutationExpected[1] = 0;
permutationExpected[2] = 2;
VectorValues deltaNewtonExpected;
deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonExpected.insert(1, Vector_(2, .4, .5));
deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
VectorValues deltaNewtonUnpermutedExpected;
deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationNewtonExpected(3);
permutationNewtonExpected[0] = 1;
permutationNewtonExpected[1] = 0;
permutationNewtonExpected[2] = 2;
Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
VectorValues deltaRgUnpermutedExpected;
deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationRgExpected(3);
permutationRgExpected[0] = 1;
permutationRgExpected[1] = 0;
permutationRgExpected[2] = 2;
Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
VectorValues deltaRgExpected;
deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgExpected.insert(1, Vector_(2, .4, .5));
deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
vector<bool> replacedKeysExpected(3, false);
Ordering orderingExpected; orderingExpected += 100, (0), (1);
Ordering orderingExpected; orderingExpected += 100, 0, 1;
ISAM2::Nodes nodesExpected(
3, ISAM2::sharedClique());
ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique());
// Expand initial state
ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
EXPECT(assert_equal(thetaExpected, theta));
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
EXPECT(assert_equal(deltaExpected, delta));
EXPECT(assert_equal(deltaNewtonExpected, deltaNewton));
EXPECT(assert_equal(deltaRgExpected, deltaRg));
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
EXPECT(assert_equal(orderingExpected, ordering));
}

10
tests/testGaussianJunctionTreeB.cpp
View File

@ -52,7 +52,7 @@ using symbol_shorthand::L;
C3 x1 : x2
C4 x7 : x6
*/
TEST( GaussianJunctionTree, constructor2 )
TEST( GaussianJunctionTreeB, constructor2 )
{
// create a graph
Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
@ -88,7 +88,7 @@ TEST( GaussianJunctionTree, constructor2 )
}
/* ************************************************************************* */
TEST( GaussianJunctionTree, optimizeMultiFrontal )
TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
{
// create a graph
GaussianFactorGraph fg;
@ -108,7 +108,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
}
/* ************************************************************************* */
TEST( GaussianJunctionTree, optimizeMultiFrontal2)
TEST( GaussianJunctionTreeB, optimizeMultiFrontal2)
{
// create a graph
example::Graph nlfg = createNonlinearFactorGraph();
@ -126,7 +126,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
}
/* ************************************************************************* */
TEST(GaussianJunctionTree, slamlike) {
TEST(GaussianJunctionTreeB, slamlike) {
Values init;
planarSLAM::Graph newfactors;
planarSLAM::Graph fullgraph;
@ -188,7 +188,7 @@ TEST(GaussianJunctionTree, slamlike) {
}
/* ************************************************************************* */
TEST(GaussianJunctionTree, simpleMarginal) {
TEST(GaussianJunctionTreeB, simpleMarginal) {
typedef BayesTree<GaussianConditional> GaussianBayesTree;

14
tests/testNonlinearEquality.cpp
View File

@ -37,7 +37,7 @@ typedef PriorFactor<Pose2> PosePrior;
typedef NonlinearEquality<Pose2> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
Symbol key('x',1);
static Symbol key('x',1);
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) {
@ -241,8 +241,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
}
/* ************************************************************************* */
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) {
@ -504,10 +504,10 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
}
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2 K(fov,w,h);
static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef visualSLAM::Graph VGraph;

8
tests/testSerializationSLAM.cpp
View File

@ -174,10 +174,10 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Point3)
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
static Point3 pt3(1.0, 2.0, 3.0);
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
static Pose3 pose3(rt3, pt3);
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
/* ************************************************************************* */
TEST (Serialization, visual_system) {

3
wrap/CMakeLists.txt
View File

@ -4,7 +4,7 @@ find_package(Boost 1.42 COMPONENTS system filesystem thread REQUIRED)
# Build the executable itself
file(GLOB wrap_srcs "*.cpp")
list(REMOVE_ITEM wrap_srcs wrap.cpp)
list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
add_library(wrap_lib STATIC ${wrap_srcs})
add_executable(wrap wrap.cpp)
target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
@ -19,7 +19,6 @@ install(FILES matlab.h DESTINATION include/wrap)
# Build tests
if (GTSAM_BUILD_TESTS)
add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}")
set(wrap_local_libs wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "")
endif(GTSAM_BUILD_TESTS)