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

Merge pull request #1099 from borglab/release/4.2a5

release/4.3a0
Frank Dellaert 2022-02-14 11:29:53 -05:00 committed by GitHub
parent d6edcea4c4 46f3a48a5b
commit a74c7dd03c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
101 changed files with 2538 additions and 1316 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -11,7 +11,7 @@ endif()
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 2)
set (GTSAM_VERSION_PATCH 0)
set (GTSAM_PRERELEASE_VERSION "a4")
set (GTSAM_PRERELEASE_VERSION "a5")
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
if (${GTSAM_VERSION_PATCH} EQUAL 0)

8
examples/SFMExampleExpressions_bal.cpp
View File

@ -28,7 +28,8 @@
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <vector>
using namespace std;
@ -46,10 +47,9 @@ int main(int argc, char* argv[]) {
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
ExpressionFactorGraph graph;

10
examples/SFMExample_bal.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SFMExample.cpp
* @file SFMExample_bal.cpp
* @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
* @author Frank Dellaert
*/
@ -20,7 +20,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <vector>
using namespace std;
@ -41,9 +42,8 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
NonlinearFactorGraph graph;

10
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -22,7 +22,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <gtsam/base/timing.h>
@ -45,10 +46,9 @@ int main(int argc, char* argv[]) {
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
NonlinearFactorGraph graph;
@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras()
mydata.numberTracks() % mydata.numberCameras()
<< endl;
tictoc_print_();

3
gtsam/base/FastSet.h
View File

@ -18,6 +18,9 @@
#pragma once
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <gtsam/base/FastDefaultAllocator.h>

1
gtsam/base/serialization.h
View File

@ -25,6 +25,7 @@
#include <string>
// includes for standard serialization types
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>

4
gtsam/discrete/DiscreteConditional.cpp
View File

@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
/* ****************************************************************************/
DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
size_t parent_value) const {
size_t frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"Single value likelihood can only be invoked on single-variable "
"conditional");
DiscreteValues values;
values.emplace(keys_[0], parent_value);
values.emplace(keys_[0], frontal);
return likelihood(values);
}

2
gtsam/discrete/DiscreteConditional.h
View File

@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
const DiscreteValues& frontalValues) const;
/** Single variable version of likelihood. */
DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
/**
* sample

19
gtsam/discrete/discrete.i
View File

@ -70,7 +70,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
bool showZero = true) const;
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
std::vector<std::pair<gtsam::DiscreteValues, double>> enumerate() const;
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
@ -97,7 +97,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
const gtsam::Ordering& orderedKeys);
gtsam::DiscreteConditional operator*(
const gtsam::DiscreteConditional& other) const;
DiscreteConditional marginal(gtsam::Key key) const;
gtsam::DiscreteConditional marginal(gtsam::Key key) const;
void print(string s = "Discrete Conditional\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
@ -269,13 +269,16 @@ class DiscreteFactorGraph {
gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type);
gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet eliminateSequential();
gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
gtsam::DiscreteBayesNet* eliminateSequential();
gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
eliminatePartialSequential(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree eliminateMultifrontal();
gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
gtsam::DiscreteBayesTree* eliminateMultifrontal();
gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
string dot(

1
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -150,7 +150,6 @@ TEST(DiscreteBayesNet, Dot) {
fragment.add((Either | Tuberculosis, LungCancer) = "F T T T");
string actual = fragment.dot();
cout << actual << endl;
EXPECT(actual ==
"digraph {\n"
" size=\"5,5\";\n"

22
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) {
string actual = self.bayesTree->dot();
EXPECT(actual ==
"digraph G{\n"
"0[label=\"13,11,6,7\"];\n"
"0[label=\"13, 11, 6, 7\"];\n"
"0->1\n"
"1[label=\"14 : 11,13\"];\n"
"1[label=\"14 : 11, 13\"];\n"
"1->2\n"
"2[label=\"9,12 : 14\"];\n"
"2[label=\"9, 12 : 14\"];\n"
"2->3\n"
"3[label=\"3 : 9,12\"];\n"
"3[label=\"3 : 9, 12\"];\n"
"2->4\n"
"4[label=\"2 : 9,12\"];\n"
"4[label=\"2 : 9, 12\"];\n"
"2->5\n"
"5[label=\"8 : 12,14\"];\n"
"5[label=\"8 : 12, 14\"];\n"
"5->6\n"
"6[label=\"1 : 8,12\"];\n"
"6[label=\"1 : 8, 12\"];\n"
"5->7\n"
"7[label=\"0 : 8,12\"];\n"
"7[label=\"0 : 8, 12\"];\n"
"1->8\n"
"8[label=\"10 : 13,14\"];\n"
"8[label=\"10 : 13, 14\"];\n"
"8->9\n"
"9[label=\"5 : 10,13\"];\n"
"9[label=\"5 : 10, 13\"];\n"
"8->10\n"
"10[label=\"4 : 10,13\"];\n"
"10[label=\"4 : 10, 13\"];\n"
"}");
}

3
gtsam/geometry/Cal3Bundler.h
View File

@ -41,6 +41,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
public:
enum { dimension = 3 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3Bundler>;
/// @name Standard Constructors
/// @{

3
gtsam/geometry/Cal3DS2.h
View File

@ -37,6 +37,9 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
public:
enum { dimension = 9 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3DS2>;
/// @name Standard Constructors
/// @{

3
gtsam/geometry/Cal3DS2_Base.h
View File

@ -47,6 +47,9 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
public:
enum { dimension = 9 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
/// @name Standard Constructors
/// @{

3
gtsam/geometry/Cal3Unified.h
View File

@ -52,6 +52,9 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
public:
enum { dimension = 10 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3Unified>;
/// @name Standard Constructors
/// @{

29
gtsam/geometry/Rot3.h
View File

@ -49,16 +49,14 @@
namespace gtsam {
/**
* @brief A 3D rotation represented as a rotation matrix if the preprocessor
* symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
* is defined.
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
private:
/**
* @brief Rot3 is a 3D rotation represented as a rotation matrix if the
* preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
* if it is defined.
* @addtogroup geometry
*/
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
private:
#ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */
@ -67,8 +65,7 @@ namespace gtsam {
SO3 rot_;
#endif
public:
public:
/// @name Constructors and named constructors
/// @{
@ -83,7 +80,7 @@ namespace gtsam {
*/
Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
/** constructor from a rotation matrix, as doubles in *row-major* order !!! */
/// Construct from a rotation matrix, as doubles in *row-major* order !!!
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33);
@ -567,6 +564,9 @@ namespace gtsam {
#endif
};
/// std::vector of Rot3s, mainly for wrapper
using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@ -585,5 +585,6 @@ namespace gtsam {
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}
} // namespace gtsam

16
gtsam/inference/BayesTree-inst.h
View File

@ -95,7 +95,7 @@ namespace gtsam {
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
const KeyFormatter& indexFormatter,
const KeyFormatter& keyFormatter,
int parentnum) const {
static int num = 0;
bool first = true;
@ -104,10 +104,10 @@ namespace gtsam {
std::string parent = out.str();
parent += "[label=\"";
for (Key index : clique->conditional_->frontals()) {
if (!first) parent += ",";
for (Key key : clique->conditional_->frontals()) {
if (!first) parent += ", ";
first = false;
parent += indexFormatter(index);
parent += keyFormatter(key);
}
if (clique->parent()) {
@ -116,10 +116,10 @@ namespace gtsam {
}
first = true;
for (Key sep : clique->conditional_->parents()) {
if (!first) parent += ",";
for (Key parentKey : clique->conditional_->parents()) {
if (!first) parent += ", ";
first = false;
parent += indexFormatter(sep);
parent += keyFormatter(parentKey);
}
parent += "\"];\n";
s << parent;
@ -127,7 +127,7 @@ namespace gtsam {
for (sharedClique c : clique->children) {
num++;
dot(s, c, indexFormatter, parentnum);
dot(s, c, keyFormatter, parentnum);
}
}

47
gtsam/inference/inference.i
View File

@ -4,6 +4,12 @@
namespace gtsam {
// Headers for overloaded methods below, break hierarchy :-/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/Key.h>
// Default keyformatter
@ -98,10 +104,41 @@ class Ordering {
Ordering();
Ordering(const gtsam::Ordering& other);
template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
gtsam::GaussianFactorGraph}>
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering ColamdConstrainedLast(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
bool forceOrder = false);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering ColamdConstrainedFirst(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
bool forceOrder = false);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
const FACTOR_GRAPH& graph);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
@ -135,12 +172,6 @@ class DotWriter {
};
#include <gtsam/inference/VariableIndex.h>
// Headers for overloaded methods below, break hierarchy :-/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
class VariableIndex {
// Standard Constructors and Named Constructors
VariableIndex();

55
gtsam/linear/GaussianBayesNet.cpp
View File

@ -26,6 +26,9 @@
using namespace std;
using namespace gtsam;
// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);
namespace gtsam {
// Instantiate base class
@ -37,28 +40,50 @@ namespace gtsam {
return Base::equals(bn, tol);
}
/* ************************************************************************* */
VectorValues GaussianBayesNet::optimize() const
{
VectorValues soln; // no missing variables -> just create an empty vector
return optimize(soln);
/* ************************************************************************ */
VectorValues GaussianBayesNet::optimize() const {
VectorValues solution; // no missing variables -> create an empty vector
return optimize(solution);
}
/* ************************************************************************* */
VectorValues GaussianBayesNet::optimize(
const VectorValues& solutionForMissing) const {
VectorValues soln(solutionForMissing); // possibly empty
VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
/** solve each node in turn in topological sort order (parents first)*/
for (auto cg: boost::adaptors::reverse(*this)) {
// solve each node in reverse topological sort order (parents first)
for (auto cg : boost::adaptors::reverse(*this)) {
// i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
soln.insert(cg->solve(soln));
// (Rii*xi + R_i*x(i+1:))./si = yi =>
// xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
solution.insert(cg->solve(solution));
}
return soln;
return solution;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const {
VectorValues result; // no missing variables -> create an empty vector
return sample(result, rng);
}
VectorValues GaussianBayesNet::sample(VectorValues result,
std::mt19937_64* rng) const {
// sample each node in reverse topological sort order (parents first)
for (auto cg : boost::adaptors::reverse(*this)) {
const VectorValues sampled = cg->sample(result, rng);
result.insert(sampled);
}
return result;
}
/* ************************************************************************ */
VectorValues GaussianBayesNet::sample() const {
return sample(&kRandomNumberGenerator);
}
VectorValues GaussianBayesNet::sample(VectorValues given) const {
return sample(given, &kRandomNumberGenerator);
}
/* ************************************************************************ */
VectorValues GaussianBayesNet::optimizeGradientSearch() const
{
gttic(GaussianBayesTree_optimizeGradientSearch);

30
gtsam/linear/GaussianBayesNet.h
View File

@ -88,11 +88,35 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
/// back-substitution
VectorValues optimize() const;
/// Version of optimize for incomplete BayesNet, needs solution for missing variables
VectorValues optimize(const VectorValues& solutionForMissing) const;
/// Version of optimize for incomplete BayesNet, given missing variables
VectorValues optimize(const VectorValues given) const;
/**
* Sample using ancestral sampling
* Example:
* std::mt19937_64 rng(42);
* auto sample = gbn.sample(&rng);
*/
VectorValues sample(std::mt19937_64* rng) const;
/**
* Sample from an incomplete BayesNet, given missing variables
* Example:
* std::mt19937_64 rng(42);
* VectorValues given = ...;
* auto sample = gbn.sample(given, &rng);
*/
VectorValues sample(VectorValues given, std::mt19937_64* rng) const;
/// Sample using ancestral sampling, use default rng
VectorValues sample() const;
/// Sample from an incomplete BayesNet, use default rng
VectorValues sample(VectorValues given) const;
/**
* Return ordering corresponding to a topological sort.

137
gtsam/linear/GaussianConditional.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Sampler.h>
#include <boost/format.hpp>
#ifdef __GNUC__
@ -34,6 +35,9 @@
#include <list>
#include <string>
// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);
using namespace std;
namespace gtsam {
@ -43,19 +47,47 @@ namespace gtsam {
Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
BaseFactor(key, R, d, sigmas), BaseConditional(1) {}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(
Key key, const Vector& d, const Matrix& R,
Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
/* ************************************************************************ */
GaussianConditional::GaussianConditional(Key key, const Vector& d,
const Matrix& R, Key parent1,
const Matrix& S,
const SharedDiagonal& sigmas)
: BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(
Key key, const Vector& d, const Matrix& R,
Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
/* ************************************************************************ */
GaussianConditional::GaussianConditional(Key key, const Vector& d,
const Matrix& R, Key parent1,
const Matrix& S, Key parent2,
const Matrix& T,
const SharedDiagonal& sigmas)
: BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
BaseConditional(1) {}
/* ************************************************************************* */
/* ************************************************************************ */
GaussianConditional GaussianConditional::FromMeanAndStddev(
Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
// |Rx + Sy - d| = |x-(Ay + b)|/sigma
const Matrix R = Matrix::Identity(b.size(), b.size());
const Matrix S = -A;
const Vector d = b;
return GaussianConditional(key, d, R, parent, S,
noiseModel::Isotropic::Sigma(b.size(), sigma));
}
/* ************************************************************************ */
GaussianConditional GaussianConditional::FromMeanAndStddev(
Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
const Vector& b, double sigma) {
// |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
const Matrix R = Matrix::Identity(b.size(), b.size());
const Matrix S = -A1;
const Matrix T = -A2;
const Vector d = b;
return GaussianConditional(key, d, R, parent1, S, parent2, T,
noiseModel::Isotropic::Sigma(b.size(), sigma));
}
/* ************************************************************************ */
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
cout << s << " Conditional density ";
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
@ -192,7 +224,88 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const VectorValues& frontalValues) const {
// Error is |Rx - (d - Sy - Tz - ...)|^2
// so when we instantiate x (which has to be completely known) we beget:
// |Sy + Tz + ... - (d - Rx)|^2
// The noise model just transfers over!
// Get frontalValues as vector
const Vector x =
frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
// Copy the augmented Jacobian matrix:
auto newAb = Ab_;
// Restrict view to parent blocks
newAb.firstBlock() += nrFrontals_;
// Update right-hand-side (last column)
auto last = newAb.matrix().cols() - 1;
const auto RR = R().triangularView<Eigen::Upper>();
newAb.matrix().col(last) -= RR * x;
// The keys now do not include the frontal keys:
KeyVector newKeys;
newKeys.reserve(nrParents());
for (auto&& key : parents()) newKeys.push_back(key);
// Hopefully second newAb copy below is optimized out...
return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
}
/* **************************************************************************/
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const Vector& frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"GaussianConditional Single value likelihood can only be invoked on "
"single-variable conditional");
VectorValues values;
values.insert(keys_[0], frontal);
return likelihood(values);
}
/* ************************************************************************ */
VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
std::mt19937_64* rng) const {
if (nrFrontals() != 1) {
throw std::invalid_argument(
"GaussianConditional::sample can only be called on single variable "
"conditionals");
}
if (!model_) {
throw std::invalid_argument(
"GaussianConditional::sample can only be called if a diagonal noise "
"model was specified at construction.");
}
VectorValues solution = solve(parentsValues);
Key key = firstFrontalKey();
const Vector& sigmas = model_->sigmas();
solution[key] += Sampler::sampleDiagonal(sigmas, rng);
return solution;
}
VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
if (nrParents() != 0)
throw std::invalid_argument(
"sample() can only be invoked on no-parent prior");
VectorValues values;
return sample(values);
}
/* ************************************************************************ */
VectorValues GaussianConditional::sample() const {
return sample(&kRandomNumberGenerator);
}
VectorValues GaussianConditional::sample(const VectorValues& given) const {
return sample(given, &kRandomNumberGenerator);
}
/* ************************************************************************ */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
void GTSAM_DEPRECATED
GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {

74
gtsam/linear/GaussianConditional.h
View File

@ -26,12 +26,15 @@
#include <gtsam/inference/Conditional.h>
#include <gtsam/linear/VectorValues.h>
#include <random> // for std::mt19937_64
namespace gtsam {
/**
* A conditional Gaussian functions as the node in a Bayes network
* A GaussianConditional functions as the node in a Bayes network.
* It has a set of parents y,z, etc. and implements a probability density on x.
* The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
* @addtogroup linear
*/
class GTSAM_EXPORT GaussianConditional :
public JacobianFactor,
@ -43,6 +46,9 @@ namespace gtsam {
typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class
typedef Conditional<BaseFactor, This> BaseConditional; ///< Typedef to our conditional base class
/// @name Constructors
/// @{
/** default constructor needed for serialization */
GaussianConditional() {}
@ -51,13 +57,14 @@ namespace gtsam {
const SharedDiagonal& sigmas = SharedDiagonal());
/** constructor with only one parent |Rx+Sy-d| */
GaussianConditional(Key key, const Vector& d, const Matrix& R,
Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
const Matrix& S,
const SharedDiagonal& sigmas = SharedDiagonal());
/** constructor with two parents |Rx+Sy+Tz-d| */
GaussianConditional(Key key, const Vector& d, const Matrix& R,
Key name1, const Matrix& S, Key name2, const Matrix& T,
const SharedDiagonal& sigmas = SharedDiagonal());
GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
const Matrix& S, Key parent2, const Matrix& T,
const SharedDiagonal& sigmas = SharedDiagonal());
/** Constructor with arbitrary number of frontals and parents.
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
@ -76,6 +83,17 @@ namespace gtsam {
const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal());
/// Construct from mean A1 p1 + b and standard deviation.
static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
Key parent, const Vector& b,
double sigma);
/// Construct from mean A1 p1 + A2 p2 + b and standard deviation.
static GaussianConditional FromMeanAndStddev(Key key, //
const Matrix& A1, Key parent1,
const Matrix& A2, Key parent2,
const Vector& b, double sigma);
/** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by
* \c first and \c last must be in increasing order, meaning that the parents of any
* conditional may not include a conditional coming before it.
@ -86,6 +104,10 @@ namespace gtsam {
template<typename ITERATOR>
static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& = "GaussianConditional",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
@ -93,6 +115,10 @@ namespace gtsam {
/** equals function */
bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
/// @}
/// @name Standard Interface
/// @{
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
@ -125,10 +151,46 @@ namespace gtsam {
/** Performs transpose backsubstition in place on values */
void solveTransposeInPlace(VectorValues& gy) const;
/** Convert to a likelihood factor by providing value before bar. */
JacobianFactor::shared_ptr likelihood(
const VectorValues& frontalValues) const;
/** Single variable version of likelihood. */
JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
/**
* Sample from conditional, zero parent version
* Example:
* std::mt19937_64 rng(42);
* auto sample = gbn.sample(&rng);
*/
VectorValues sample(std::mt19937_64* rng) const;
/**
* Sample from conditional, given missing variables
* Example:
* std::mt19937_64 rng(42);
* VectorValues given = ...;
* auto sample = gbn.sample(given, &rng);
*/
VectorValues sample(const VectorValues& parentsValues,
std::mt19937_64* rng) const;
/// Sample, use default rng
VectorValues sample() const;
/// Sample with given values, use default rng
VectorValues sample(const VectorValues& parentsValues) const;
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated
/// @{
/** Scale the values in \c gy according to the sigmas for the frontal variables in this
* conditional. */
void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
/// @}
#endif
private:

13
gtsam/linear/GaussianDensity.cpp
View File

@ -23,9 +23,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma)
{
return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma);
GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
const Vector& mean,
double sigma) {
return GaussianDensity(key, mean,
Matrix::Identity(mean.size(), mean.size()),
noiseModel::Isotropic::Sigma(mean.size(), sigma));
}
/* ************************************************************************* */
@ -35,8 +38,8 @@ namespace gtsam {
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
cout << endl;
gtsam::print(Matrix(R()), "R: ");
gtsam::print(Vector(d()), "d: ");
gtsam::print(mean(), "mean: ");
gtsam::print(covariance(), "covariance: ");
if(model_)
model_->print("Noise model: ");
}

10
gtsam/linear/GaussianDensity.h
View File

@ -24,11 +24,10 @@
namespace gtsam {
/**
* A Gaussian density.
*
* It is implemented as a GaussianConditional without parents.
* A GaussianDensity is a GaussianConditional without parents.
* The negative log-probability is given by \f$ |Rx - d|^2 \f$
* with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$
* @addtogroup linear
*/
class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
@ -52,8 +51,9 @@ namespace gtsam {
GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
GaussianConditional(key, d, R, noiseModel) {}
/// Construct using a mean and variance
static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma);
/// Construct using a mean and standard deviation
static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
double sigma);
/// print
void print(const std::string& = "GaussianDensity",

18
gtsam/linear/Sampler.cpp
View File

@ -22,14 +22,18 @@ namespace gtsam {
/* ************************************************************************* */
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
uint_fast64_t seed)
: model_(model), generator_(seed) {}
: model_(model), generator_(seed) {
if (!model) {
throw std::invalid_argument("Sampler::Sampler needs a non-null model.");
}
}
/* ************************************************************************* */
Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
/* ************************************************************************* */
Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) {
size_t d = sigmas.size();
Vector result(d);
for (size_t i = 0; i < d; i++) {
@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
if (sigma == 0.0) {
result(i) = 0.0;
} else {
typedef std::normal_distribution<double> Normal;
Normal dist(0.0, sigma);
result(i) = dist(generator_);
std::normal_distribution<double> dist(0.0, sigma);
result(i) = dist(*rng);
}
}
return result;
}
/* ************************************************************************* */
Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
return sampleDiagonal(sigmas, &generator_);
}
/* ************************************************************************* */
Vector Sampler::sample() const {
assert(model_.get());

12
gtsam/linear/Sampler.h
View File

@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler {
/// @name access functions
/// @{
size_t dim() const {
assert(model_.get());
return model_->dim();
}
size_t dim() const { return model_->dim(); }
Vector sigmas() const {
assert(model_.get());
return model_->sigmas();
}
Vector sigmas() const { return model_->sigmas(); }
const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler {
/// sample from distribution
Vector sample() const;
/// sample with given random number generator
static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
/// @}
protected:

93
gtsam/linear/VectorValues.cpp
View File

@ -33,7 +33,7 @@ namespace gtsam {
using boost::adaptors::map_values;
using boost::accumulate;
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
{
// Merge using predicate for comparing first of pair
@ -44,7 +44,7 @@ namespace gtsam {
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Dims& dims) {
using Pair = pair<const Key, size_t>;
size_t j = 0;
@ -61,7 +61,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0;
for (const SlotEntry& v : scatter) {
@ -74,7 +74,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::Zero(const VectorValues& other)
{
VectorValues result;
@ -87,7 +87,7 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
@ -97,7 +97,7 @@ namespace gtsam {
return result.first;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::update(const VectorValues& values)
{
iterator hint = begin();
@ -115,7 +115,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::insert(const VectorValues& values)
{
size_t originalSize = size();
@ -124,14 +124,14 @@ namespace gtsam {
throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::setZero()
{
for(Vector& v: values_ | map_values)
v.setZero();
}
/* ************************************************************************* */
/* ************************************************************************ */
GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
// Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
@ -150,7 +150,7 @@ namespace gtsam {
return os;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::print(const string& str,
const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n";
@ -158,7 +158,7 @@ namespace gtsam {
cout.flush();
}
/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
@ -170,7 +170,7 @@ namespace gtsam {
return true;
}
/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector() const {
// Count dimensions
DenseIndex totalDim = 0;
@ -187,7 +187,7 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector(const Dims& keys) const
{
// Count dimensions
@ -203,12 +203,12 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);
}
/* ************************************************************************* */
/* ************************************************************************ */
namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValues::value_type,
@ -219,14 +219,14 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::hasSameStructure(const VectorValues other) const
{
return accumulate(combine(*this, other)
| transformed(internal::structureCompareOp), true, logical_and<bool>());
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::dot(const VectorValues& v) const
{
if(this->size() != v.size())
@ -244,12 +244,12 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::norm() const {
return std::sqrt(this->squaredNorm());
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::squaredNorm() const {
double sumSquares = 0.0;
using boost::adaptors::map_values;
@ -258,7 +258,7 @@ namespace gtsam {
return sumSquares;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator+(const VectorValues& c) const
{
if(this->size() != c.size())
@ -278,13 +278,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::add(const VectorValues& c) const
{
return *this + c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator+=(const VectorValues& c)
{
if(this->size() != c.size())
@ -301,13 +301,13 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace(const VectorValues& c)
{
return *this += c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace_(const VectorValues& c)
{
for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
@ -320,7 +320,7 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator-(const VectorValues& c) const
{
if(this->size() != c.size())
@ -340,13 +340,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::subtract(const VectorValues& c) const
{
return *this - c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues operator*(const double a, const VectorValues &v)
{
VectorValues result;
@ -359,13 +359,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::scale(const double a) const
{
return a * *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator*=(double alpha)
{
for(Vector& v: *this | map_values)
@ -373,12 +373,43 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::scaleInPlace(double alpha)
{
return *this *= alpha;
}
/* ************************************************************************* */
/* ************************************************************************ */
string VectorValues::html(const KeyFormatter& keyFormatter) const {
stringstream ss;
// Print out preamble.
ss << "<div>\n<table class='VectorValues'>\n <thead>\n";
// Print out header row.
ss << " <tr><th>Variable</th><th>value</th></tr>\n";
// Finish header and start body.
ss << " </thead>\n <tbody>\n";
// Print out all rows.
#ifdef GTSAM_USE_TBB
// TBB uses un-ordered map, so inefficiently order them:
std::map<Key, Vector> ordered;
for (const auto& kv : *this) ordered.emplace(kv);
for (const auto& kv : ordered) {
#else
for (const auto& kv : *this) {
#endif
ss << " <tr>";
ss << "<th>" << keyFormatter(kv.first) << "</th><td>"
<< kv.second.transpose() << "</td>";
ss << "</tr>\n";
}
ss << " </tbody>\n</table>\n</div>";
return ss.str();
}
/* ************************************************************************ */
} // \namespace gtsam

15
gtsam/linear/VectorValues.h
View File

@ -34,7 +34,7 @@
namespace gtsam {
/**
* This class represents a collection of vector-valued variables associated
* VectorValues represents a collection of vector-valued variables associated
* each with a unique integer index. It is typically used to store the variables
* of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet
* returns this class.
@ -69,7 +69,7 @@ namespace gtsam {
* which is a view on the underlying data structure.
*
* This class is additionally used in gradient descent and dog leg to store the gradient.
* \nosubgrouping
* @addtogroup linear
*/
class GTSAM_EXPORT VectorValues {
protected:
@ -344,11 +344,16 @@ namespace gtsam {
/// @}
/// @}
/// @name Matlab syntactic sugar for linear algebra operations
/// @name Wrapper support
/// @{
//inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }
/**
* @brief Output as a html table.
*
* @param keyFormatter function that formats keys.
*/
std::string html(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}

76
gtsam/linear/linear.i
View File

@ -255,6 +255,7 @@ class VectorValues {
// enabling serialization functionality
void serialize() const;
string html() const;
};
#include <gtsam/linear/GaussianFactor.h>
@ -407,8 +408,10 @@ class GaussianFactorGraph {
// Elimination and marginals
gtsam::GaussianBayesNet* eliminateSequential();
gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::GaussianBayesTree* eliminateMultifrontal();
gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
@ -466,15 +469,36 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T);
// Standard Interface
// Named constructors
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A,
gtsam::Key parent,
const Vector& b,
double sigma);
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A1,
gtsam::Key parent1,
const Matrix& A2,
gtsam::Key parent2,
const Vector& b,
double sigma);
// Testable
void print(string s = "GaussianConditional",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
// Standard Interface
gtsam::Key firstFrontalKey() const;
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::JacobianFactor* likelihood(
const gtsam::VectorValues& frontalValues) const;
gtsam::JacobianFactor* likelihood(Vector frontal) const;
gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
gtsam::VectorValues sample() const;
// Advanced Interface
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
const gtsam::VectorValues& rhs) const;
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
@ -488,14 +512,21 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
#include <gtsam/linear/GaussianDensity.h>
virtual class GaussianDensity : gtsam::GaussianConditional {
//Constructors
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
// Constructors
GaussianDensity(gtsam::Key key, Vector d, Matrix R,
const gtsam::noiseModel::Diagonal* sigmas);
//Standard Interface
static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key,
const Vector& mean,
double sigma);
// Testable
void print(string s = "GaussianDensity",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianDensity &cg, double tol) const;
bool equals(const gtsam::GaussianDensity& cg, double tol) const;
// Standard Interface
Vector mean() const;
Matrix covariance() const;
};
@ -512,6 +543,21 @@ virtual class GaussianBayesNet {
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
size_t size() const;
// Standard interface
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues given) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues sample(gtsam::VectorValues given) const;
gtsam::VectorValues sample() const;
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
// FactorGraph derived interface
gtsam::GaussianConditional* at(size_t idx) const;
gtsam::KeySet keys() const;
@ -520,21 +566,12 @@ virtual class GaussianBayesNet {
void saveGraph(const string& s) const;
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
std::pair<Matrix, Vector> matrix() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
double determinant() const;
double logDeterminant() const;
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -556,7 +593,12 @@ virtual class GaussianBayesTree {
size_t size() const;
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;

27
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -16,10 +16,12 @@
*/
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
@ -35,6 +37,7 @@ using namespace boost::assign;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
static const Key _x_ = 11, _y_ = 22, _z_ = 33;
@ -138,6 +141,30 @@ TEST( GaussianBayesNet, optimize3 )
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianBayesNet, sample) {
GaussianBayesNet gbn;
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 mean(20, 40), b(10, 10);
const double sigma = 0.01;
gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma));
gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
auto actual = gbn.sample();
EXPECT_LONGS_EQUAL(2, actual.size());
EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma));
EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma));
// Use a specific random generator
std::mt19937_64 rng(4242);
auto actual3 = gbn.sample(&rng);
EXPECT_LONGS_EQUAL(2, actual.size());
// regression is not repeatable across platforms/versions :-(
// EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
// EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
}
/* ************************************************************************* */
TEST(GaussianBayesNet, ordering)
{

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

@ -20,9 +20,10 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/assign/std/list.hpp>
@ -38,6 +39,7 @@
using namespace gtsam;
using namespace std;
using namespace boost::assign;
using symbol_shorthand::X;
static const double tol = 1e-5;
@ -316,5 +318,87 @@ TEST( GaussianConditional, isGaussianFactor ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
// Test FromMeanAndStddev named constructors
TEST(GaussianConditional, FromMeanAndStddev) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
const double sigma = 3;
VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2);
auto conditional1 =
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma;
double expected1 = 0.5 * e1.dot(e1);
EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
X(2), b, sigma);
Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
double expected2 = 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
}
/* ************************************************************************* */
// Test likelihood method (conversion to JacobianFactor)
TEST(GaussianConditional, likelihood) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 b(20, 40), x0(1, 2);
const double sigma = 0.01;
// |x0 - A1 x1 - b|^2
auto conditional =
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
VectorValues frontalValues;
frontalValues.insert(X(0), x0);
auto actual1 = conditional.likelihood(frontalValues);
CHECK(actual1);
// |(-A1) x1 - (b - x0)|^2
JacobianFactor expected(X(1), -A1, b - x0,
noiseModel::Isotropic::Sigma(2, sigma));
EXPECT(assert_equal(expected, *actual1, tol));
// Check single vector version
auto actual2 = conditional.likelihood(x0);
CHECK(actual2);
EXPECT(assert_equal(expected, *actual2, tol));
}
/* ************************************************************************* */
// Test sampling
TEST(GaussianConditional, sample) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 b(20, 40), x1(3, 4);
const double sigma = 0.01;
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
auto actual1 = density.sample();
EXPECT_LONGS_EQUAL(1, actual1.size());
EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma));
VectorValues given;
given.insert(X(1), x1);
auto conditional =
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
auto actual2 = conditional.sample(given);
EXPECT_LONGS_EQUAL(1, actual2.size());
EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma));
// Use a specific random generator
std::mt19937_64 rng(4242);
auto actual3 = conditional.sample(given, &rng);
EXPECT_LONGS_EQUAL(1, actual2.size());
// regression is not repeatable across platforms/versions :-(
// EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

19
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -17,10 +17,13 @@
**/
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace std;
using symbol_shorthand::X;
/* ************************************************************************* */
TEST(GaussianDensity, constructor)
@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor)
EXPECT(assert_equal(s, copied.get_model()->sigmas()));
}
/* ************************************************************************* */
// Test FromMeanAndStddev named constructor
TEST(GaussianDensity, FromMeanAndStddev) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 b(20, 40), x0(1, 2);
const double sigma = 3;
VectorValues values;
values.insert(X(0), x0);
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
Vector2 e = (x0 - b) / sigma;
double expected = 0.5 * e.dot(e);
EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

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

@ -17,7 +17,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
@ -248,6 +248,33 @@ TEST(VectorValues, print)
EXPECT(expected == actual.str());
}
/* ************************************************************************* */
// Check html representation.
TEST(VectorValues, html) {
VectorValues vv;
using symbol_shorthand::X;
vv.insert(X(1), Vector2(2, 3.1));
vv.insert(X(2), Vector2(4, 5.2));
vv.insert(X(5), Vector2(6, 7.3));
vv.insert(X(7), Vector2(8, 9.4));
string expected =
"<div>\n"
"<table class='VectorValues'>\n"
" <thead>\n"
" <tr><th>Variable</th><th>value</th></tr>\n"
" </thead>\n"
" <tbody>\n"
" <tr><th>x1</th><td> 2 3.1</td></tr>\n"
" <tr><th>x2</th><td> 4 5.2</td></tr>\n"
" <tr><th>x5</th><td> 6 7.3</td></tr>\n"
" <tr><th>x7</th><td> 8 9.4</td></tr>\n"
" </tbody>\n"
"</table>\n"
"</div>";
string actual = vv.html();
EXPECT(actual == expected);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

4
gtsam/navigation/ImuBias.cpp
View File

@ -66,8 +66,8 @@ namespace imuBias {
// }
/// ostream operator
std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
os << "acc = " << Point3(bias.accelerometer());
os << " gyro = " << Point3(bias.gyroscope());
os << "acc = " << bias.accelerometer().transpose();
os << " gyro = " << bias.gyroscope().transpose();
return os;
}

27
gtsam/nonlinear/ISAM2Params.h
View File

@ -300,18 +300,10 @@ struct GTSAM_EXPORT ISAM2Params {
RelinearizationThreshold getRelinearizeThreshold() const {
return relinearizeThreshold;
}
int getRelinearizeSkip() const { return relinearizeSkip; }
bool isEnableRelinearization() const { return enableRelinearization; }
bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
std::string getFactorization() const {
return factorizationTranslator(factorization);
}
bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
KeyFormatter getKeyFormatter() const { return keyFormatter; }
bool isEnableDetailedResults() const { return enableDetailedResults; }
bool isEnablePartialRelinearizationCheck() const {
return enablePartialRelinearizationCheck;
}
void setOptimizationParams(OptimizationParams optimizationParams) {
this->optimizationParams = optimizationParams;
@ -319,31 +311,12 @@ struct GTSAM_EXPORT ISAM2Params {
void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
this->relinearizeThreshold = relinearizeThreshold;
}
void setRelinearizeSkip(int relinearizeSkip) {
this->relinearizeSkip = relinearizeSkip;
}
void setEnableRelinearization(bool enableRelinearization) {
this->enableRelinearization = enableRelinearization;
}
void setEvaluateNonlinearError(bool evaluateNonlinearError) {
this->evaluateNonlinearError = evaluateNonlinearError;
}
void setFactorization(const std::string& factorization) {
this->factorization = factorizationTranslator(factorization);
}
void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
this->cacheLinearizedFactors = cacheLinearizedFactors;
}
void setKeyFormatter(KeyFormatter keyFormatter) {
this->keyFormatter = keyFormatter;
}
void setEnableDetailedResults(bool enableDetailedResults) {
this->enableDetailedResults = enableDetailedResults;
}
void setEnablePartialRelinearizationCheck(
bool enablePartialRelinearizationCheck) {
this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
}
GaussianFactorGraph::Eliminate getEliminationFunction() const {
return factorization == CHOLESKY

28
gtsam/nonlinear/nonlinear.i
View File

@ -98,11 +98,11 @@ class NonlinearFactorGraph {
string dot(
const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& formatting = GraphvizFormatting());
const GraphvizFormatting& writer = GraphvizFormatting());
void saveGraph(
const string& s, const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& formatting = GraphvizFormatting()) const;
const GraphvizFormatting& writer = GraphvizFormatting()) const;
// enabling serialization functionality
void serialize() const;
@ -588,21 +588,19 @@ class ISAM2Params {
void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
void setRelinearizeThreshold(double threshold);
void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
int getRelinearizeSkip() const;
void setRelinearizeSkip(int relinearizeSkip);
bool isEnableRelinearization() const;
void setEnableRelinearization(bool enableRelinearization);
bool isEvaluateNonlinearError() const;
void setEvaluateNonlinearError(bool evaluateNonlinearError);
string getFactorization() const;
void setFactorization(string factorization);
bool isCacheLinearizedFactors() const;
void setCacheLinearizedFactors(bool cacheLinearizedFactors);
bool isEnableDetailedResults() const;
void setEnableDetailedResults(bool enableDetailedResults);
bool isEnablePartialRelinearizationCheck() const;
void setEnablePartialRelinearizationCheck(
bool enablePartialRelinearizationCheck);
int relinearizeSkip;
bool enableRelinearization;
bool evaluateNonlinearError;
bool cacheLinearizedFactors;
bool enableDetailedResults;
bool enablePartialRelinearizationCheck;
bool findUnusedFactorSlots;
enum Factorization { CHOLESKY, QR };
Factorization factorization;
};
class ISAM2Clique {

11
gtsam/sam/sam.i
View File

@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
void serialize() const;
};
// between points:
typedef gtsam::RangeFactor<gtsam::Point2, gtsam::Point2> RangeFactor2;
typedef gtsam::RangeFactor<gtsam::Point3, gtsam::Point3> RangeFactor3;
// between pose and point:
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
// between poses:
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
// more specialized types:
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>

460
gtsam/sfm/SfmData.cpp Normal file
View File

@ -0,0 +1,460 @@
/* ----------------------------------------------------------------------------
* 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 SfmData.cpp
* @date January 2022
* @author Frank dellaert
* @brief Data structure for dealing with Structure from Motion data
*/
#include <gtsam/inference/Symbol.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <fstream>
#include <iostream>
namespace gtsam {
using std::cout;
using std::endl;
using gtsam::symbol_shorthand::P;
/* ************************************************************************** */
void SfmData::print(const std::string &s) const {
std::cout << "Number of cameras = " << cameras.size() << std::endl;
std::cout << "Number of tracks = " << tracks.size() << std::endl;
}
/* ************************************************************************** */
bool SfmData::equals(const SfmData &sfmData, double tol) const {
// check number of cameras and tracks
if (cameras.size() != sfmData.cameras.size() ||
tracks.size() != sfmData.tracks.size()) {
return false;
}
// check each camera
for (size_t i = 0; i < cameras.size(); ++i) {
if (!camera(i).equals(sfmData.camera(i), tol)) {
return false;
}
}
// check each track
for (size_t j = 0; j < tracks.size(); ++j) {
if (!track(j).equals(sfmData.track(j), tol)) {
return false;
}
}
return true;
}
/* ************************************************************************* */
Rot3 openGLFixedRotation() { // this is due to different convention for
// cameras in gtsam and openGL
/* R = [ 1 0 0
* 0 -1 0
* 0 0 -1]
*/
Matrix3 R_mat = Matrix3::Zero(3, 3);
R_mat(0, 0) = 1.0;
R_mat(1, 1) = -1.0;
R_mat(2, 2) = -1.0;
return Rot3(R_mat);
}
/* ************************************************************************* */
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
Rot3 R90 = openGLFixedRotation();
Rot3 wRc = (R.inverse()).compose(R90);
// Our camera-to-world translation wTc = -R'*t
return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
Rot3 R90 = openGLFixedRotation();
Rot3 cRw_openGL = R90.compose(R.inverse());
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
return Pose3(cRw_openGL, t_openGL);
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
PoseGTSAM.z());
}
/* ************************************************************************** */
SfmData SfmData::FromBundlerFile(const std::string &filename) {
// Load the data file
std::ifstream is(filename.c_str(), std::ifstream::in);
if (!is) {
throw std::runtime_error(
"Error in FromBundlerFile: can not find the file!!");
}
SfmData sfmData;
// Ignore the first line
char aux[500];
is.getline(aux, 500);
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints;
is >> nrPoses >> nrPoints;
// Get the information for the camera poses
for (size_t i = 0; i < nrPoses; i++) {
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
// Get the rotation matrix
float r11, r12, r13;
float r21, r22, r23;
float r31, r32, r33;
is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix
Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
// Check for all-zero R, in which case quit
if (r11 == 0 && r12 == 0 && r13 == 0) {
throw std::runtime_error(
"Error in FromBundlerFile: zero rotation matrix");
}
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
sfmData.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
sfmData.tracks.reserve(nrPoints);
for (size_t j = 0; j < nrPoints; j++) {
SfmTrack track;
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
track.p = Point3(x, y, z);
// Get the color information
float r, g, b;
is >> r >> g >> b;
track.r = r / 255.f;
track.g = g / 255.f;
track.b = b / 255.f;
// Now get the visibility information
size_t nvisible = 0;
is >> nvisible;
track.measurements.reserve(nvisible);
track.siftIndices.reserve(nvisible);
for (size_t k = 0; k < nvisible; k++) {
size_t cam_idx = 0, point_idx = 0;
float u, v;
is >> cam_idx >> point_idx >> u >> v;
track.measurements.emplace_back(cam_idx, Point2(u, -v));
track.siftIndices.emplace_back(cam_idx, point_idx);
}
sfmData.tracks.push_back(track);
}
return sfmData;
}
/* ************************************************************************** */
SfmData SfmData::FromBalFile(const std::string &filename) {
// Load the data file
std::ifstream is(filename.c_str(), std::ifstream::in);
if (!is) {
throw std::runtime_error("Error in FromBalFile: can not find the file!!");
}
SfmData sfmData;
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints, nrObservations;
is >> nrPoses >> nrPoints >> nrObservations;
sfmData.tracks.resize(nrPoints);
// Get the information for the observations
for (size_t k = 0; k < nrObservations; k++) {
size_t i = 0, j = 0;
float u, v;
is >> i >> j >> u >> v;
sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v));
}
// Get the information for the camera poses
for (size_t i = 0; i < nrPoses; i++) {
// Get the Rodrigues vector
float wx, wy, wz;
is >> wx >> wy >> wz;
Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
sfmData.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
for (size_t j = 0; j < nrPoints; j++) {
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
SfmTrack &track = sfmData.tracks[j];
track.p = Point3(x, y, z);
track.r = 0.4f;
track.g = 0.4f;
track.b = 0.4f;
}
return sfmData;
}
/* ************************************************************************** */
bool writeBAL(const std::string &filename, const SfmData &data) {
// Open the output file
std::ofstream os;
os.open(filename.c_str());
os.precision(20);
if (!os.is_open()) {
cout << "Error in writeBAL: can not open the file!!" << endl;
return false;
}
// Write the number of camera poses and 3D points
size_t nrObservations = 0;
for (size_t j = 0; j < data.tracks.size(); j++) {
nrObservations += data.tracks[j].numberMeasurements();
}
// Write observations
os << data.cameras.size() << " " << data.tracks.size() << " "
<< nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
const SfmTrack &track = data.tracks[j];
for (size_t k = 0; k < track.numberMeasurements();
k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().px();
double v0 = data.cameras[i].calibration().py();
if (u0 != 0 || v0 != 0) {
cout << "writeBAL has not been tested for calibration with nonzero "
"(u0,v0)"
<< endl;
}
double pixelBALx = track.measurements[k].second.x() -
u0; // center of image is the origin
double pixelBALy = -(track.measurements[k].second.y() -
v0); // center of image is the origin
Point2 pixelMeasurement(pixelBALx, pixelBALy);
os << i /*camera id*/ << " " << j /*point id*/ << " "
<< pixelMeasurement.x() /*u of the pixel*/ << " "
<< pixelMeasurement.y() /*v of the pixel*/ << endl;
}
}
os << endl;
// Write cameras
for (size_t i = 0; i < data.cameras.size(); i++) { // for each camera
Pose3 poseGTSAM = data.cameras[i].pose();
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
os << poseOpenGL.translation().x() << endl;
os << poseOpenGL.translation().y() << endl;
os << poseOpenGL.translation().z() << endl;
os << cameraCalibration.fx() << endl;
os << cameraCalibration.k1() << endl;
os << cameraCalibration.k2() << endl;
os << endl;
}
// Write the points
for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
Point3 point = data.tracks[j].p;
os << point.x() << endl;
os << point.y() << endl;
os << point.z() << endl;
os << endl;
}
os.close();
return true;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
bool readBundler(const std::string &filename, SfmData &data) {
try {
data = SfmData::FromBundlerFile(filename);
return true;
} catch (const std::exception & /* e */) {
return false;
}
}
bool readBAL(const std::string &filename, SfmData &data) {
try {
data = SfmData::FromBalFile(filename);
return true;
} catch (const std::exception & /* e */) {
return false;
}
}
#endif
SfmData readBal(const std::string &filename) {
return SfmData::FromBalFile(filename);
}
/* ************************************************************************** */
bool writeBALfromValues(const std::string &filename, const SfmData &data,
const Values &values) {
using Camera = PinholeCamera<Cal3Bundler>;
SfmData dataValues = data;
// Store poses or cameras in SfmData
size_t nrPoses = values.count<Pose3>();
if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera
Pose3 pose = values.at<Pose3>(i);
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
}
} else {
size_t nrCameras = values.count<Camera>();
if (nrCameras == dataValues.cameras.size()) { // we only estimated camera
// poses and calibration
for (size_t i = 0; i < nrCameras; i++) { // for each camera
Key cameraKey = i; // symbol('c',i);
Camera camera = values.at<Camera>(cameraKey);
dataValues.cameras[i] = camera;
}
} else {
cout << "writeBALfromValues: different number of cameras in "
"SfM_dataValues (#cameras "
<< dataValues.cameras.size() << ") and values (#cameras " << nrPoses
<< ", #poses " << nrCameras << ")!!" << endl;
return false;
}
}
// Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.tracks.size();
if (nrPoints != nrTracks) {
cout << "writeBALfromValues: different number of points in "
"SfM_dataValues (#points= "
<< nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
}
for (size_t j = 0; j < nrTracks; j++) { // for each point
Key pointKey = P(j);
if (values.exists(pointKey)) {
Point3 point = values.at<Point3>(pointKey);
dataValues.tracks[j].p = point;
} else {
dataValues.tracks[j].r = 1.0;
dataValues.tracks[j].g = 0.0;
dataValues.tracks[j].b = 0.0;
dataValues.tracks[j].p = Point3(0, 0, 0);
}
}
// Write SfmData to file
return writeBAL(filename, dataValues);
}
/* ************************************************************************** */
NonlinearFactorGraph SfmData::generalSfmFactors(
const SharedNoiseModel &model) const {
using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
NonlinearFactorGraph factors;
size_t j = 0;
for (const SfmTrack &track : tracks) {
for (const SfmMeasurement &m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
factors.emplace_shared<ProjectionFactor>(uv, model, i, P(j));
}
j += 1;
}
return factors;
}
/* ************************************************************************** */
NonlinearFactorGraph SfmData::sfmFactorGraph(
const SharedNoiseModel &model, boost::optional<size_t> fixedCamera,
boost::optional<size_t> fixedPoint) const {
using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
NonlinearFactorGraph graph = generalSfmFactors(model);
using noiseModel::Constrained;
if (fixedCamera) {
graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9));
}
if (fixedPoint) {
graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
}
return graph;
}
/* ************************************************************************** */
Values initialCamerasEstimate(const SfmData &db) {
Values initial;
size_t i = 0; // NO POINTS: j = 0;
for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
return initial;
}
/* ************************************************************************** */
Values initialCamerasAndPointsEstimate(const SfmData &db) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
return initial;
}
/* ************************************************************************** */
} // namespace gtsam

236
gtsam/sfm/SfmData.h Normal file
View File

@ -0,0 +1,236 @@
/* ----------------------------------------------------------------------------
* 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 SfmData.h
* @date January 2022
* @author Frank dellaert
* @brief Data structure for dealing with Structure from Motion data
*/
#pragma once
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h>
#include <string>
#include <vector>
namespace gtsam {
/// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfmCamera;
/**
* @brief SfmData stores a bunch of SfmTracks
* @addtogroup sfm
*/
struct GTSAM_EXPORT SfmData {
std::vector<SfmCamera> cameras; ///< Set of cameras
std::vector<SfmTrack> tracks; ///< Sparse set of points
/// @name Create from file
/// @{
/**
* @brief Parses a bundler output file and return result as SfmData instance.
* @param filename The name of the bundler file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
static SfmData FromBundlerFile(const std::string& filename);
/**
* @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return
* result as SfmData instance.
* @param filename The name of the BAL file.
* @return SfM structure where the data is stored.
*/
static SfmData FromBalFile(const std::string& filename);
/// @}
/// @name Standard Interface
/// @{
/// Add a track to SfmData
void addTrack(const SfmTrack& t) { tracks.push_back(t); }
/// Add a camera to SfmData
void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
/// The number of reconstructed 3D points
size_t numberTracks() const { return tracks.size(); }
/// The number of cameras
size_t numberCameras() const { return cameras.size(); }
/// The track formed by series of landmark measurements
SfmTrack track(size_t idx) const { return tracks[idx]; }
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const { return cameras[idx]; }
/**
* @brief Create projection factors using keys i and P(j)
*
* @param model a noise model for projection errors
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph generalSfmFactors(
const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
1.0)) const;
/**
* @brief Create factor graph with projection factors and gauge fix.
*
* Note: pose keys are simply integer indices, points use Symbol('p', j).
*
* @param model a noise model for projection errors
* @param fixedCamera which camera to fix, if any (use boost::none if none)
* @param fixedPoint which point to fix, if any (use boost::none if none)
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph sfmFactorGraph(
const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
boost::optional<size_t> fixedCamera = 0,
boost::optional<size_t> fixedPoint = 0) const;
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const SfmData& sfmData, double tol = 1e-9) const;
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated
/// @{
void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
cameras.push_back(cam);
}
size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
/// @}
#endif
/// @name Serialization
/// @{
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(cameras);
ar& BOOST_SERIALIZATION_NVP(tracks);
}
/// @}
};
/// traits
template <>
struct traits<SfmData> : public Testable<SfmData> {};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
SfmData& data);
GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
SfmData& data);
#endif
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
* returns the data as a SfmData structure. Mainly used by wrapped code.
* @param filename The name of the BAL file.
* @return SfM structure where the data is stored.
*/
GTSAM_EXPORT SfmData readBal(const std::string& filename);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
* from a SfmData structure
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
* from a SfmData structure and a value structure (measurements are the same as
* the SfM input data, while camera poses and values are read from Values)
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored (values can be
* either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
* Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
* and "Symbol::('p',j)" for landmark j.
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfmData& data, const Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose
* @param R rotation in openGL
* @param tx x component of the translation in openGL
* @param ty y component of the translation in openGL
* @param tz z component of the translation in openGL
* @return Pose3 in GTSAM format
*/
GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param R rotation in GTSAM
* @param tx x component of the translation in GTSAM
* @param ty y component of the translation in GTSAM
* @param tz z component of the translation in GTSAM
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param PoseGTSAM pose in GTSAM format
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
/**
* @brief This function creates initial values for cameras from db.
*
* No symbol is used, so camera keys are simply integer indices.
*
* @param SfmData
* @return Values
*/
GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
/**
* @brief This function creates initial values for cameras and points from db
*
* Note: Pose keys are simply integer indices, points use Symbol('p', j).
*
* @param SfmData
* @return Values
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
} // namespace gtsam

71
gtsam/sfm/SfmTrack.cpp Normal file
View File

@ -0,0 +1,71 @@
/* ----------------------------------------------------------------------------
* 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 SfmTrack.cpp
* @date January 2022
* @author Frank Dellaert
* @brief A simple data structure for a track in Structure from Motion
*/
#include <gtsam/sfm/SfmTrack.h>
#include <iostream>
namespace gtsam {
void SfmTrack::print(const std::string& s) const {
std::cout << "Track with " << measurements.size();
std::cout << " measurements of point " << p << std::endl;
}
bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const {
// check the 3D point
if (!p.isApprox(sfmTrack.p)) {
return false;
}
// check the RGB values
if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) {
return false;
}
// compare size of vectors for measurements and siftIndices
if (numberMeasurements() != sfmTrack.numberMeasurements() ||
siftIndices.size() != sfmTrack.siftIndices.size()) {
return false;
}
// compare measurements (order sensitive)
for (size_t idx = 0; idx < numberMeasurements(); ++idx) {
SfmMeasurement measurement = measurements[idx];
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
if (measurement.first != otherMeasurement.first ||
!measurement.second.isApprox(otherMeasurement.second)) {
return false;
}
}
// compare sift indices (order sensitive)
for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
SiftIndex index = siftIndices[idx];
SiftIndex otherIndex = sfmTrack.siftIndices[idx];
if (index.first != otherIndex.first || index.second != otherIndex.second) {
return false;
}
}
return true;
}
} // namespace gtsam

133
gtsam/sfm/SfmTrack.h Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* 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 SfmTrack.h
* @date January 2022
* @author Frank Dellaert
* @brief A simple data structure for a track in Structure from Motion
*/
#pragma once
#include <gtsam/base/serialization.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <string>
#include <utility>
#include <vector>
namespace gtsam {
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfmMeasurement;
/// Sift index for SfmTrack
typedef std::pair<size_t, size_t> SiftIndex;
/**
* @brief An SfmTrack stores SfM measurements grouped in a track
* @addtogroup sfm
*/
struct GTSAM_EXPORT SfmTrack {
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
/// The 2D image projections (id,(u,v))
std::vector<SfmMeasurement> measurements;
/// The feature descriptors
std::vector<SiftIndex> siftIndices;
/// @name Constructors
/// @{
explicit SfmTrack(float r = 0, float g = 0, float b = 0)
: p(0, 0, 0), r(r), g(g), b(b) {}
explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
float b = 0)
: p(pt), r(r), g(g), b(b) {}
/// @}
/// @name Standard Interface
/// @{
/// Add measurement (camera_idx, Point2) to track
void addMeasurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
/// Total number of measurements in this track
size_t numberMeasurements() const { return measurements.size(); }
/// Get the measurement (camera index, Point2) at pose index `idx`
const SfmMeasurement& measurement(size_t idx) const {
return measurements[idx];
}
/// Get the SIFT feature index corresponding to the measurement at `idx`
const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
/// Get 3D point
const Point3& point3() const { return p; }
/// Get RGB values describing 3d point
Point3 rgb() const { return Point3(r, g, b); }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated
/// @{
void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
size_t GTSAM_DEPRECATED number_measurements() const {
return measurements.size();
}
/// @}
#endif
/// @name Serialization
/// @{
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(p);
ar& BOOST_SERIALIZATION_NVP(r);
ar& BOOST_SERIALIZATION_NVP(g);
ar& BOOST_SERIALIZATION_NVP(b);
ar& BOOST_SERIALIZATION_NVP(measurements);
ar& BOOST_SERIALIZATION_NVP(siftIndices);
}
/// @}
};
template <typename T>
struct traits;
template <>
struct traits<SfmTrack> : public Testable<SfmTrack> {};
} // namespace gtsam

2
gtsam/sfm/ShonanAveraging.h
View File

@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging {
size_t nrUnknowns() const { return nrUnknowns_; }
/// Return number of measurements
size_t nrMeasurements() const { return measurements_.size(); }
size_t numberMeasurements() const { return measurements_.size(); }
/// k^th binary measurement
const BinaryMeasurement<Rot> &measurement(size_t k) const {

61
gtsam/sfm/sfm.i
View File

@ -4,7 +4,62 @@
namespace gtsam {
// #####
#include <gtsam/sfm/SfmTrack.h>
class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t numberMeasurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void addMeasurement(size_t idx, const gtsam::Point2& m);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
};
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename);
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const;
size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
gtsam::NonlinearFactorGraph sfmFactorGraph(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
size_t fixedCamera = 0, size_t fixedPoint = 0) const;
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
#include <gtsam/sfm/ShonanFactor.h>
@ -92,7 +147,7 @@ class ShonanAveraging2 {
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
size_t numberMeasurements() const;
gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i);
@ -140,7 +195,7 @@ class ShonanAveraging3 {
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
size_t numberMeasurements() const;
gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i);

214
gtsam/sfm/tests/testSfmData.cpp Normal file
View File

@ -0,0 +1,214 @@
/* ----------------------------------------------------------------------------
* 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 TestSfmData.cpp
* @date January 2022
* @author Frank dellaert
* @brief tests for SfmData class and associated utilites
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/SfmData.h>
using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::P;
namespace gtsam {
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
} // namespace gtsam
/* ************************************************************************* */
TEST(dataSet, Balbianello) {
// The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfmData sfmData = SfmData::FromBundlerFile(filename);
// Check number of things
EXPECT_LONGS_EQUAL(5, sfmData.numberCameras());
EXPECT_LONGS_EQUAL(544, sfmData.numberTracks());
const SfmTrack& track0 = sfmData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = sfmData.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 1));
// We share *one* noiseModel between all projection factors
auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Convert to NonlinearFactorGraph
NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model);
EXPECT_LONGS_EQUAL(1419, graph.size()); // regression
// Get initial estimate
Values values = initialCamerasAndPointsEstimate(sfmData);
EXPECT_LONGS_EQUAL(549, values.size()); // regression
}
/* ************************************************************************* */
TEST(dataSet, readBAL_Dubrovnik) {
// The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData sfmData = SfmData::FromBalFile(filename);
// Check number of things
EXPECT_LONGS_EQUAL(3, sfmData.numberCameras());
EXPECT_LONGS_EQUAL(7, sfmData.numberTracks());
const SfmTrack& track0 = sfmData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = sfmData.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 12));
}
/* ************************************************************************* */
TEST(dataSet, openGL2gtsam) {
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0, 0.0, 0.0);
Pose3 poseGTSAM = Pose3(R, t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); // columns!
Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(),
-r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc, t);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(dataSet, gtsam2openGL) {
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(1.0, 20.0, 10.0);
Pose3 actual = Pose3(R, t);
Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
Pose3 expected = gtsam2openGL(poseGTSAM);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(dataSet, writeBAL_Dubrovnik) {
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData = SfmData::FromBalFile(filenameToRead);
// Write readData to file filenameToWrite
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks());
for (size_t i = 0; i < readData.numberCameras(); i++) {
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
EXPECT(assert_equal(expectedCamera, actualCamera));
}
for (size_t j = 0; j < readData.numberTracks(); j++) {
// check point
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack actualTrack = readData.tracks[j];
Point3 expectedPoint = expectedTrack.p;
Point3 actualPoint = actualTrack.p;
EXPECT(assert_equal(expectedPoint, actualPoint));
// check rgb
Point3 expectedRGB =
Point3(expectedTrack.r, expectedTrack.g, expectedTrack.b);
Point3 actualRGB = Point3(actualTrack.r, actualTrack.g, actualTrack.b);
EXPECT(assert_equal(expectedRGB, actualRGB));
// check measurements
for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) {
EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,
actualTrack.measurements[k].first);
EXPECT(assert_equal(expectedTrack.measurements[k].second,
actualTrack.measurements[k].second));
}
}
}
/* ************************************************************************* */
TEST(dataSet, writeBALfromValues_Dubrovnik) {
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData = SfmData::FromBalFile(filenameToRead);
Pose3 poseChange =
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
Values values;
for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
values.insert(i, pose);
}
for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point
Point3 point = poseChange.transformFrom(readData.tracks[j].p);
values.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, values);
// Read the file we wrote
SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
// Check that the reprojection errors are the same and the poses are correct
// Check number of things
EXPECT_LONGS_EQUAL(3, writtenData.numberCameras());
EXPECT_LONGS_EQUAL(7, writtenData.numberTracks());
const SfmTrack& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = writtenData.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 12));
Pose3 expectedPose = camera0.pose();
Pose3 actualPose = values.at<Pose3>(0);
EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = values.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/slam/FrobeniusFactor.h
View File

@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
@ -106,6 +108,8 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @name Constructor
/// @{

3
gtsam/slam/PoseRotationPrior.h
View File

@ -39,6 +39,9 @@ protected:
public:
/** default constructor - only use for serialization */
PoseRotationPrior() {}
/** standard constructor */
PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
: Base(model, key), measured_(rot_z) {}

2
gtsam/slam/SmartFactorBase.h
View File

@ -146,7 +146,7 @@ protected:
*/
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
}

358
gtsam/slam/dataset.cpp
View File

@ -54,8 +54,6 @@
using namespace std;
namespace fs = boost::filesystem;
using gtsam::symbol_shorthand::L;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
#define LINESIZE 81920
@ -208,15 +206,15 @@ std::map<size_t, Point2> parseVariables<Point2>(const std::string &filename,
/* ************************************************************************* */
// Interpret noise parameters according to flags
static SharedNoiseModel
createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
static SharedNoiseModel createNoiseModel(
const Vector6 &v, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
if (noiseFormat == NoiseFormatAUTO) {
// Try to guess covariance matrix layout
if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
noiseFormat = NoiseFormatGRAPH;
} else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
} else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
noiseFormat = NoiseFormatCOV;
} else {
@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) {
return make_pair(graph, initial);
}
/* ************************************************************************* */
Rot3 openGLFixedRotation() { // this is due to different convention for
// cameras in gtsam and openGL
/* R = [ 1 0 0
* 0 -1 0
* 0 0 -1]
*/
Matrix3 R_mat = Matrix3::Zero(3, 3);
R_mat(0, 0) = 1.0;
R_mat(1, 1) = -1.0;
R_mat(2, 2) = -1.0;
return Rot3(R_mat);
}
/* ************************************************************************* */
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
Rot3 R90 = openGLFixedRotation();
Rot3 wRc = (R.inverse()).compose(R90);
// Our camera-to-world translation wTc = -R'*t
return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
Rot3 R90 = openGLFixedRotation();
Rot3 cRw_openGL = R90.compose(R.inverse());
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
return Pose3(cRw_openGL, t_openGL);
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
PoseGTSAM.z());
}
/* ************************************************************************* */
bool readBundler(const string &filename, SfmData &data) {
// Load the data file
ifstream is(filename.c_str(), ifstream::in);
if (!is) {
cout << "Error in readBundler: can not find the file!!" << endl;
return false;
}
// Ignore the first line
char aux[500];
is.getline(aux, 500);
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints;
is >> nrPoses >> nrPoints;
// Get the information for the camera poses
for (size_t i = 0; i < nrPoses; i++) {
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
// Get the rotation matrix
float r11, r12, r13;
float r21, r22, r23;
float r31, r32, r33;
is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix
Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
// Check for all-zero R, in which case quit
if (r11 == 0 && r12 == 0 && r13 == 0) {
cout << "Error in readBundler: zero rotation matrix for pose " << i
<< endl;
return false;
}
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
data.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
data.tracks.reserve(nrPoints);
for (size_t j = 0; j < nrPoints; j++) {
SfmTrack track;
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
track.p = Point3(x, y, z);
// Get the color information
float r, g, b;
is >> r >> g >> b;
track.r = r / 255.f;
track.g = g / 255.f;
track.b = b / 255.f;
// Now get the visibility information
size_t nvisible = 0;
is >> nvisible;
track.measurements.reserve(nvisible);
track.siftIndices.reserve(nvisible);
for (size_t k = 0; k < nvisible; k++) {
size_t cam_idx = 0, point_idx = 0;
float u, v;
is >> cam_idx >> point_idx >> u >> v;
track.measurements.emplace_back(cam_idx, Point2(u, -v));
track.siftIndices.emplace_back(cam_idx, point_idx);
}
data.tracks.push_back(track);
}
is.close();
return true;
}
/* ************************************************************************* */
bool readBAL(const string &filename, SfmData &data) {
// Load the data file
ifstream is(filename.c_str(), ifstream::in);
if (!is) {
cout << "Error in readBAL: can not find the file!!" << endl;
return false;
}
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints, nrObservations;
is >> nrPoses >> nrPoints >> nrObservations;
data.tracks.resize(nrPoints);
// Get the information for the observations
for (size_t k = 0; k < nrObservations; k++) {
size_t i = 0, j = 0;
float u, v;
is >> i >> j >> u >> v;
data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
}
// Get the information for the camera poses
for (size_t i = 0; i < nrPoses; i++) {
// Get the Rodrigues vector
float wx, wy, wz;
is >> wx >> wy >> wz;
Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
data.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
for (size_t j = 0; j < nrPoints; j++) {
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
SfmTrack &track = data.tracks[j];
track.p = Point3(x, y, z);
track.r = 0.4f;
track.g = 0.4f;
track.b = 0.4f;
}
is.close();
return true;
}
/* ************************************************************************* */
SfmData readBal(const string &filename) {
SfmData data;
readBAL(filename, data);
return data;
}
/* ************************************************************************* */
bool writeBAL(const string &filename, SfmData &data) {
// Open the output file
ofstream os;
os.open(filename.c_str());
os.precision(20);
if (!os.is_open()) {
cout << "Error in writeBAL: can not open the file!!" << endl;
return false;
}
// Write the number of camera poses and 3D points
size_t nrObservations = 0;
for (size_t j = 0; j < data.number_tracks(); j++) {
nrObservations += data.tracks[j].number_measurements();
}
// Write observations
os << data.number_cameras() << " " << data.number_tracks() << " "
<< nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
const SfmTrack &track = data.tracks[j];
for (size_t k = 0; k < track.number_measurements();
k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().px();
double v0 = data.cameras[i].calibration().py();
if (u0 != 0 || v0 != 0) {
cout << "writeBAL has not been tested for calibration with nonzero "
"(u0,v0)"
<< endl;
}
double pixelBALx = track.measurements[k].second.x() -
u0; // center of image is the origin
double pixelBALy = -(track.measurements[k].second.y() -
v0); // center of image is the origin
Point2 pixelMeasurement(pixelBALx, pixelBALy);
os << i /*camera id*/ << " " << j /*point id*/ << " "
<< pixelMeasurement.x() /*u of the pixel*/ << " "
<< pixelMeasurement.y() /*v of the pixel*/ << endl;
}
}
os << endl;
// Write cameras
for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
Pose3 poseGTSAM = data.cameras[i].pose();
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
os << poseOpenGL.translation().x() << endl;
os << poseOpenGL.translation().y() << endl;
os << poseOpenGL.translation().z() << endl;
os << cameraCalibration.fx() << endl;
os << cameraCalibration.k1() << endl;
os << cameraCalibration.k2() << endl;
os << endl;
}
// Write the points
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
Point3 point = data.tracks[j].p;
os << point.x() << endl;
os << point.y() << endl;
os << point.z() << endl;
os << endl;
}
os.close();
return true;
}
bool writeBALfromValues(const string &filename, const SfmData &data,
Values &values) {
using Camera = PinholeCamera<Cal3Bundler>;
SfmData dataValues = data;
// Store poses or cameras in SfmData
size_t nrPoses = values.count<Pose3>();
if (nrPoses ==
dataValues.number_cameras()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.number_cameras();
i++) { // for each camera
Pose3 pose = values.at<Pose3>(X(i));
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
}
} else {
size_t nrCameras = values.count<Camera>();
if (nrCameras == dataValues.number_cameras()) { // we only estimated camera
// poses and calibration
for (size_t i = 0; i < nrCameras; i++) { // for each camera
Key cameraKey = i; // symbol('c',i);
Camera camera = values.at<Camera>(cameraKey);
dataValues.cameras[i] = camera;
}
} else {
cout << "writeBALfromValues: different number of cameras in "
"SfM_dataValues (#cameras "
<< dataValues.number_cameras() << ") and values (#cameras "
<< nrPoses << ", #poses " << nrCameras << ")!!" << endl;
return false;
}
}
// Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(),
nrTracks = dataValues.number_tracks();
if (nrPoints != nrTracks) {
cout << "writeBALfromValues: different number of points in "
"SfM_dataValues (#points= "
<< nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
}
for (size_t j = 0; j < nrTracks; j++) { // for each point
Key pointKey = P(j);
if (values.exists(pointKey)) {
Point3 point = values.at<Point3>(pointKey);
dataValues.tracks[j].p = point;
} else {
dataValues.tracks[j].r = 1.0;
dataValues.tracks[j].g = 0.0;
dataValues.tracks[j].b = 0.0;
dataValues.tracks[j].p = Point3(0, 0, 0);
}
}
// Write SfmData to file
return writeBAL(filename, dataValues);
}
Values initialCamerasEstimate(const SfmData &db) {
Values initial;
size_t i = 0; // NO POINTS: j = 0;
for (const SfmCamera &camera : db.cameras)
initial.insert(i++, camera);
return initial;
}
Values initialCamerasAndPointsEstimate(const SfmData &db) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera &camera : db.cameras)
initial.insert((i++), camera);
for (const SfmTrack &track : db.tracks)
initial.insert(P(j++), track.p);
return initial;
}
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
BetweenFactorPose2s
parse2DFactors(const std::string &filename,

295
gtsam/slam/dataset.h
View File

@ -22,6 +22,7 @@
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
@ -167,10 +168,11 @@ GTSAM_EXPORT GraphAndValues load2D(
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT GraphAndValues
load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
size_t maxIndex = 0, bool addNoise = false, bool smart = true,
NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -185,8 +187,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT GraphAndValues
readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
* @brief This function writes a g2o file from
@ -206,286 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfmMeasurement;
/// Sift index for SfmTrack
typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points
struct SfmTrack {
SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {}
SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {}
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SiftIndex> siftIndices;
/// Get RGB values describing 3d point
const Point3 rgb() const { return Point3(r, g, b); }
/// Total number of measurements in this track
size_t number_measurements() const {
return measurements.size();
}
/// Get the measurement (camera index, Point2) at pose index `idx`
SfmMeasurement measurement(size_t idx) const {
return measurements[idx];
}
/// Get the SIFT feature index corresponding to the measurement at `idx`
SiftIndex siftIndex(size_t idx) const {
return siftIndices[idx];
}
/// Get 3D point
const Point3& point3() const {
return p;
}
/// Add measurement (camera_idx, Point2) to track
void add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(p);
ar & BOOST_SERIALIZATION_NVP(r);
ar & BOOST_SERIALIZATION_NVP(g);
ar & BOOST_SERIALIZATION_NVP(b);
ar & BOOST_SERIALIZATION_NVP(measurements);
ar & BOOST_SERIALIZATION_NVP(siftIndices);
}
/// assert equality up to a tolerance
bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
// check the 3D point
if (!p.isApprox(sfmTrack.p)) {
return false;
}
// check the RGB values
if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
return false;
}
// compare size of vectors for measurements and siftIndices
if (number_measurements() != sfmTrack.number_measurements() ||
siftIndices.size() != sfmTrack.siftIndices.size()) {
return false;
}
// compare measurements (order sensitive)
for (size_t idx = 0; idx < number_measurements(); ++idx) {
SfmMeasurement measurement = measurements[idx];
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
if (measurement.first != otherMeasurement.first ||
!measurement.second.isApprox(otherMeasurement.second)) {
return false;
}
}
// compare sift indices (order sensitive)
for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
SiftIndex index = siftIndices[idx];
SiftIndex otherIndex = sfmTrack.siftIndices[idx];
if (index.first != otherIndex.first ||
index.second != otherIndex.second) {
return false;
}
}
return true;
}
/// print
void print(const std::string& s = "") const {
std::cout << "Track with " << measurements.size();
std::cout << " measurements of point " << p << std::endl;
}
};
/* ************************************************************************* */
/// traits
template<>
struct traits<SfmTrack> : public Testable<SfmTrack> {
};
/// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfmCamera;
/// Define the structure for SfM data
struct SfmData {
std::vector<SfmCamera> cameras; ///< Set of cameras
std::vector<SfmTrack> tracks; ///< Sparse set of points
size_t number_cameras() const {
return cameras.size();
}
/// The number of reconstructed 3D points
size_t number_tracks() const {
return tracks.size();
}
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const {
return cameras[idx];
}
/// The track formed by series of landmark measurements
SfmTrack track(size_t idx) const {
return tracks[idx];
}
/// Add a track to SfmData
void add_track(const SfmTrack& t) {
tracks.push_back(t);
}
/// Add a camera to SfmData
void add_camera(const SfmCamera& cam) {
cameras.push_back(cam);
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(cameras);
ar & BOOST_SERIALIZATION_NVP(tracks);
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const SfmData &sfmData, double tol = 1e-9) const {
// check number of cameras and tracks
if (number_cameras() != sfmData.number_cameras() ||
number_tracks() != sfmData.number_tracks()) {
return false;
}
// check each camera
for (size_t i = 0; i < number_cameras(); ++i) {
if (!camera(i).equals(sfmData.camera(i), tol)) {
return false;
}
}
// check each track
for (size_t j = 0; j < number_tracks(); ++j) {
if (!track(j).equals(sfmData.track(j), tol)) {
return false;
}
}
return true;
}
/// print
void print(const std::string& s = "") const {
std::cout << "Number of cameras = " << number_cameras() << std::endl;
std::cout << "Number of tracks = " << number_tracks() << std::endl;
}
};
/* ************************************************************************* */
/// traits
template<>
struct traits<SfmData> : public Testable<SfmData> {
};
/**
* @brief This function parses a bundler output file and stores the data into a
* SfmData structure
* @param filename The name of the bundler file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfmData structure
* @param filename The name of the BAL file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data
* as a SfmData structure. Mainly used by wrapped code.
* @param filename The name of the BAL file.
* @return SfM structure where the data is stored.
*/
GTSAM_EXPORT SfmData readBal(const std::string& filename);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfmData structure
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfmData structure and a value structure (measurements are the same as the SfM input data,
* while camera poses and values are read from Values)
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
* cameras, and should be Point3 for the 3D points). Note that the current version
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfmData &data, Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose
* @param R rotation in openGL
* @param tx x component of the translation in openGL
* @param ty y component of the translation in openGL
* @param tz z component of the translation in openGL
* @return Pose3 in GTSAM format
*/
GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param R rotation in GTSAM
* @param tx x component of the translation in GTSAM
* @param ty y component of the translation in GTSAM
* @param tz z component of the translation in GTSAM
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param PoseGTSAM pose in GTSAM format
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
/**
* @brief This function creates initial values for cameras from db
* @param SfmData
* @return Values
*/
GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
/**
* @brief This function creates initial values for cameras and points from db
* @param SfmData
* @return Values
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose2s

75
gtsam/slam/slam.i
View File

@ -209,61 +209,27 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
#include <gtsam/slam/dataset.h>
class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(size_t idx, const gtsam::Point2& m);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
enum NoiseFormat {
NoiseFormatG2O,
NoiseFormatTORO,
NoiseFormatGRAPH,
NoiseFormatCOV,
NoiseFormatAUTO
};
class SfmData {
SfmData();
size_t number_cameras() const;
size_t number_tracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void add_track(const gtsam::SfmTrack& t);
void add_camera(const gtsam::SfmCamera& cam);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
enum KernelFunctionType {
KernelFunctionTypeNONE,
KernelFunctionTypeHUBER,
KernelFunctionTypeTUKEY
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model = nullptr,
size_t maxIndex = 0, bool addNoise = false, bool smart = true,
gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO,
gtsam::KernelFunctionType kernelFunctionType =
gtsam::KernelFunctionType::KernelFunctionTypeNONE);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
@ -290,9 +256,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
bool is3D);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
string filename, const bool is3D = false,
gtsam::KernelFunctionType kernelFunctionType =
gtsam::KernelFunctionType::KernelFunctionTypeNONE);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
@ -323,6 +290,8 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
};
gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
size_t d);

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

@ -151,27 +151,6 @@ TEST(dataSet, load2DVictoriaPark) {
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
}
/* ************************************************************************* */
TEST( dataSet, Balbianello)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfmData mydata;
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */
TEST(dataSet, readG2o3D) {
const string g2oFile = findExampleDataFile("pose3example");
@ -461,160 +440,6 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise)
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
}
/* ************************************************************************* */
TEST( dataSet, openGL2gtsam)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0,0.0,0.0);
Pose3 poseGTSAM = Pose3(R,t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
Rot3 cRw(
r1.x(), r2.x(), r3.x(),
-r1.y(), -r2.y(), -r3.y(),
-r1.z(), -r2.z(), -r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc,t);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, gtsam2openGL)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(1.0,20.0,10.0);
Pose3 actual = Pose3(R,t);
Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
Pose3 expected = gtsam2openGL(poseGTSAM);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, writeBAL_Dubrovnik)
{
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras());
EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks());
for (size_t i = 0; i < readData.number_cameras(); i++){
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
EXPECT(assert_equal(expectedCamera,actualCamera));
}
for (size_t j = 0; j < readData.number_tracks(); j++){
// check point
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack actualTrack = readData.tracks[j];
Point3 expectedPoint = expectedTrack.p;
Point3 actualPoint = actualTrack.p;
EXPECT(assert_equal(expectedPoint,actualPoint));
// check rgb
Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b );
Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b);
EXPECT(assert_equal(expectedRGB,actualRGB));
// check measurements
for (size_t k = 0; k < actualTrack.number_measurements(); k++){
EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first);
EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
}
}
}
/* ************************************************************************* */
TEST( dataSet, writeBALfromValues_Dubrovnik){
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(X(i), pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote
SfmData writtenData;
readBAL(filenameToWrite, writtenData);
// Check that the reprojection errors are the same and the poses are correct
// Check number of things
EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
const SfmTrack& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfmCamera& camera0 = writtenData.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
Pose3 expectedPose = camera0.pose();
Pose3 actualPose = value.at<Pose3>(X(0));
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = value.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -15,6 +15,7 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
using namespace std::placeholders;
@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
namespace example1 {
const string filename = findExampleDataFile("18pointExample1.txt");
SfmData data;
bool readOK = readBAL(filename, data);
SfmData data = SfmData::FromBalFile(filename);
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation();
// TODO: maybe default value not good; assert with 0th
@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); }
//*************************************************************************
TEST(EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
namespace example2 {
const string filename = findExampleDataFile("5pointExample2.txt");
SfmData data;
bool readOK = readBAL(filename, data);
SfmData data = SfmData::FromBalFile(filename);
Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation();
EssentialMatrix trueE(aRb, Unit3(aTb));
@ -632,14 +629,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++)
for (size_t i = 0; i < data.numberTracks(); i++)
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
K);
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) {
for (size_t i = 0; i < data.numberTracks(); i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
@ -654,7 +651,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++)
for (size_t i = 0; i < data.numberTracks(); i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result

7
gtsam/slam/tests/testSerializationDataset.cpp
View File

@ -16,6 +16,7 @@
* @date Jan 1, 2021
*/
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/base/serializationTestHelpers.h>
@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers;
TEST(dataSet, sfmDataSerialization) {
// Test the serialization of SfmData
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmData mydata = SfmData::FromBalFile(filename);
// round-trip equality check on serialization and subsequent deserialization
EXPECT(equalsObj(mydata));
@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) {
TEST(dataSet, sfmTrackSerialization) {
// Test the serialization of SfmTrack
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmData mydata = SfmData::FromBalFile(filename);
SfmTrack track = mydata.track(0);

15
gtsam_unstable/gtsam_unstable.i
View File

@ -566,7 +566,13 @@ virtual class FixedLagSmoother {
gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
double smootherLag() const;
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps);
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
const gtsam::FactorIndices &factorsToRemove);
gtsam::Values calculateEstimate() const;
};
@ -576,6 +582,8 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
BatchFixedLagSmoother(double smootherLag);
BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
void print(string s = "BatchFixedLagSmoother:\n") const;
gtsam::LevenbergMarquardtParams params() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@ -589,7 +597,12 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother(double smootherLag);
IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
void print(string s = "IncrementalFixedLagSmoother:\n") const;
gtsam::ISAM2Params params() const;
gtsam::NonlinearFactorGraph getFactors() const;
gtsam::ISAM2 getISAM2() const;
};
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>

3
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -113,6 +113,9 @@ public:
/// Get results of latest isam2 update
const ISAM2Result& getISAM2Result() const{ return isamResult_; }
/// Get the iSAM2 object which is used for the inference internally
const ISAM2& getISAM2() const { return isam_; }
protected:
/** Create default parameters */

2
matlab/+gtsam/VisualISAMInitialize.m
View File

@ -7,7 +7,7 @@ import gtsam.*
%% Initialize iSAM
params = gtsam.ISAM2Params;
if options.alwaysRelinearize
params.setRelinearizeSkip(1);
params.relinearizeSkip = 1;
end
isam = ISAM2(params);

2
matlab/CMakeLists.txt
View File

@ -68,6 +68,8 @@ set(interface_files
${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
${GTSAM_SOURCE_DIR}/gtsam/base/base.i
${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
${PROJECT_SOURCE_DIR}/gtsam/inference/inference.i
${PROJECT_SOURCE_DIR}/gtsam/discrete/discrete.i
${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i

2
matlab/gtsam_examples/IMUKittiExampleGPS.m
View File

@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis);
%% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY');
isamParams.setRelinearizeSkip(10);
isamParams.relinearizeSkip = 10;
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;

2
matlab/unstable_examples/+imuSimulator/IMUComparison.m
View File

@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isamParams.relinearizeSkip = 1;
isam = gtsam.ISAM2(isamParams);
initialValues = Values;

2
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
View File

@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isamParams.relinearizeSkip = 1;
isam = gtsam.ISAM2(isamParams);
sigma_init_x = 1.0;

2
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -119,7 +119,7 @@ h = figure;
% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY');
isamParams.setRelinearizeSkip(10);
isamParams.relinearizeSkip = 10;
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;

2
matlab/unstable_examples/IMUKittiExampleAdvanced.m
View File

@ -82,7 +82,7 @@ w_coriolis = [0;0;0];
%% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('QR');
isamParams.setRelinearizeSkip(1);
isamParams.relinearizeSkip = 1;
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;

2
matlab/unstable_examples/IMUKittiExampleVO.m
View File

@ -58,7 +58,7 @@ w_coriolis = [0;0;0];
%% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY');
isamParams.setRelinearizeSkip(10);
isamParams.relinearizeSkip = 10;
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;

1
python/CMakeLists.txt
View File

@ -45,6 +45,7 @@ set(ignore
gtsam::Point3Pairs
gtsam::Pose3Pairs
gtsam::Pose3Vector
gtsam::Rot3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3
gtsam::DiscreteKey

2
python/gtsam/examples/IMUKittiExampleGPS.py
View File

@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement],
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.setRelinearizeSkip(10)
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)

2
python/gtsam/examples/Pose2ISAM2Example.py
View File

@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example():
# update calls are required to perform the relinearization.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.1)
parameters.setRelinearizeSkip(1)
parameters.relinearizeSkip = 1
isam = gtsam.ISAM2(parameters)
# Create the ground truth odometry measurements of the robot during the trajectory.

2
python/gtsam/examples/Pose3ISAM2Example.py
View File

@ -140,7 +140,7 @@ def Pose3_ISAM2_example():
# update calls are required to perform the relinearization.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.1)
parameters.setRelinearizeSkip(1)
parameters.relinearizeSkip = 1
isam = gtsam.ISAM2(parameters)
# Create the ground truth poses of the robot trajectory.

7
python/gtsam/examples/SFMExample.py
View File

@ -8,7 +8,6 @@ See LICENSE for the license information
A structure-from-motion problem on a simulated dataset
"""
from __future__ import print_function
import gtsam
import matplotlib.pyplot as plt
@ -89,7 +88,7 @@ def main():
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
graph.print('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
@ -100,7 +99,7 @@ def main():
for j, point in enumerate(points):
transformed_point = point + 0.1*np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
initial_estimate.print('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
@ -108,7 +107,7 @@ def main():
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
result.print('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))

35
python/gtsam/examples/SFMExample_bal.py
View File

@ -15,9 +15,9 @@ import logging
import sys
import gtsam
from gtsam import (GeneralSFMFactorCal3Bundler,
from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
from gtsam.symbol_shorthand import C, P # type: ignore
from gtsam.symbol_shorthand import P # type: ignore
from gtsam.utils import plot # type: ignore
from matplotlib import pyplot as plt
@ -26,13 +26,12 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for cam_idx in range(scene_data.number_cameras()):
plot_vals.insert(C(cam_idx),
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
for j in range(scene_data.number_tracks()):
for i in range(scene_data.numberCameras()):
plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j)))
plot.plot_3d_points(0, plot_vals, linespec="g.")
@ -46,9 +45,9 @@ def run(args: argparse.Namespace) -> None:
input_file = args.input_file
# Load the SfM data from file
scene_data = gtsam.readBal(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(),
scene_data.number_cameras())
scene_data = SfmData.FromBalFile(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.numberCameras())
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
@ -57,19 +56,19 @@ def run(args: argparse.Namespace) -> None:
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph
for j in range(scene_data.number_tracks()):
for j in range(scene_data.numberTracks()):
track = scene_data.track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
for m_idx in range(track.numberMeasurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0),
0, scene_data.camera(0),
gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
@ -82,13 +81,13 @@ def run(args: argparse.Namespace) -> None:
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.number_cameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
for i in range(scene_data.numberCameras()):
camera = scene_data.camera(i)
initial.insert(i, camera)
i += 1
# add each SfmTrack
for j in range(scene_data.number_tracks()):
for j in range(scene_data.numberTracks()):
track = scene_data.track(j)
initial.insert(P(j), track.point3())

8
python/gtsam/examples/SimpleRotation.py
View File

@ -31,7 +31,7 @@ def main():
- A measurement model with the correct dimensionality for the factor
"""
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
prior.print('goal angle')
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)
@ -48,7 +48,7 @@ def main():
"""
graph = gtsam.NonlinearFactorGraph()
graph.push_back(factor)
graph.print_('full graph')
graph.print('full graph')
"""
Step 3: Create an initial estimate
@ -65,7 +65,7 @@ def main():
"""
initial = gtsam.Values()
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
initial.print_('initial estimate')
initial.print('initial estimate')
"""
Step 4: Optimize
@ -77,7 +77,7 @@ def main():
with the final state of the optimization.
"""
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print_('final result')
result.print('final result')
if __name__ == '__main__':

2
python/gtsam/examples/VisualISAM2Example.py
View File

@ -81,7 +81,7 @@ def visual_ISAM2_example():
# will approach the batch result.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.01)
parameters.setRelinearizeSkip(1)
parameters.relinearizeSkip = 1
isam = gtsam.ISAM2(parameters)
# Create a Factor Graph and Values to hold the new data

4
python/gtsam/examples/VisualISAMExample.py
View File

@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
This version uses iSAM to solve the problem incrementally
"""
from __future__ import print_function
import numpy as np
import gtsam
from gtsam.examples import SFMdata
@ -94,7 +92,7 @@ def main():
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
current_estimate.print('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)

3
python/gtsam/preamble/inference.h
View File

@ -11,5 +11,4 @@
* mutations on Python side will not be reflected on C++.
*/
#include <pybind11/stl.h>
#include <pybind11/stl.h>

1
python/gtsam/preamble/slam.h
View File

@ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector);

5
python/gtsam/specializations/slam.h
View File

@ -12,8 +12,9 @@
*/
py::bind_vector<
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(
m_, "BetweenFactorPose3s");
py::bind_vector<
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(
m_, "BetweenFactorPose2s");
py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector");

35
python/gtsam/tests/test_DiscreteBayesNet.py
View File

@ -12,7 +12,9 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name
import unittest
import textwrap
import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase
@ -126,6 +128,39 @@ class TestDiscreteBayesNet(GtsamTestCase):
actual = fragment.sample(given)
self.assertEqual(len(actual), 5)
def test_dot(self):
"""Check that dot works with position hints."""
fragment = DiscreteBayesNet()
fragment.add(Either, [Tuberculosis, LungCancer], "F T T T")
MyAsia = gtsam.symbol('a', 0), 2 # use a symbol!
fragment.add(Tuberculosis, [MyAsia], "99/1 95/5")
fragment.add(LungCancer, [Smoking], "99/1 90/10")
# Make sure we can *update* position hints
writer = gtsam.DotWriter()
ph: dict = writer.positionHints
ph.update({'a': 2}) # hint at symbol position
writer.positionHints = ph
# Check the output of dot
actual = fragment.dot(writer=writer)
expected_result = """\
digraph {
size="5,5";
var3[label="3"];
var4[label="4"];
var5[label="5"];
var6[label="6"];
vara0[label="a0", pos="0,2!"];
var4->var6
vara0->var3
var3->var5
var6->var5
}"""
self.assertEqual(actual, textwrap.dedent(expected_result))
if __name__ == "__main__":
unittest.main()

53
python/gtsam/tests/test_GaussianBayesNet.py Normal file
View File

@ -0,0 +1,53 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for Gaussian Bayes Nets.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import GaussianBayesNet, GaussianConditional
from gtsam.utils.test_case import GtsamTestCase
# some keys
_x_ = 11
_y_ = 22
_z_ = 33
def smallBayesNet():
"""Create a small Bayes Net for testing"""
bayesNet = GaussianBayesNet()
I_1x1 = np.eye(1, dtype=float)
bayesNet.push_back(GaussianConditional(
_x_, [9.0], I_1x1, _y_, I_1x1))
bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1))
return bayesNet
class TestGaussianBayesNet(GtsamTestCase):
"""Tests for Gaussian Bayes nets."""
def test_matrix(self):
"""Test matrix method"""
R, d = smallBayesNet().matrix() # get matrix and RHS
R1 = np.array([
[1.0, 1.0],
[0.0, 1.0]])
d1 = np.array([9.0, 5.0])
np.testing.assert_equal(R, R1)
np.testing.assert_equal(d, d1)
if __name__ == '__main__':
unittest.main()

6
python/gtsam/tests/test_GraphvizFormatting.py
View File

@ -78,7 +78,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X
graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))
def test_factor_points(self):
@ -100,7 +100,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.plotFactorPoints = False
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))
def test_width_height(self):
@ -127,7 +127,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.figureHeightInches = 10
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))

40
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -15,27 +15,15 @@ import unittest
import gtsam
import numpy as np
from gtsam import Rot3
from gtsam.utils.test_case import GtsamTestCase
KEY = 0
MODEL = gtsam.noiseModel.Unit.Create(3)
def find_Karcher_mean_Rot3(rotations):
"""Find the Karcher mean of given values."""
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
# No closed form solution.
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, gtsam.Rot3())
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
return result.atRot3(KEY)
# Rot3 version
R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
R = Rot3.Expmap(np.array([0.1, 0, 0]))
class TestKarcherMean(GtsamTestCase):
@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase):
def test_find(self):
# Check that optimizing for Karcher mean (which minimizes Between distance)
# gets correct result.
rotations = {R, R.inverse()}
expected = gtsam.Rot3()
actual = find_Karcher_mean_Rot3(rotations)
rotations = gtsam.Rot3Vector([R, R.inverse()])
expected = Rot3()
actual = gtsam.FindKarcherMean(rotations)
self.gtsamAssertEquals(expected, actual)
def test_find_karcher_mean_identity(self):
"""Averaging 3 identity rotations should yield the identity."""
a1Rb1 = Rot3()
a2Rb2 = Rot3()
a3Rb3 = Rot3()
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list)
self.gtsamAssertEquals(aRb, aRb_expected)
def test_factor(self):
"""Check that the InnerConstraint factor leaves the mean unchanged."""
# Make a graph with two variables, one between, and one InnerConstraint
@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase):
initial = gtsam.Values()
initial.insert(1, R.inverse())
initial.insert(2, R)
expected = find_Karcher_mean_Rot3([R, R.inverse()])
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = find_Karcher_mean_Rot3(
[result.atRot3(1), result.atRot3(2)])
actual = gtsam.FindKarcherMean(
gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))

90
python/gtsam/tests/test_SfmData.py
View File

@ -14,9 +14,9 @@ from __future__ import print_function
import unittest
import numpy as np
import gtsam
import numpy as np
from gtsam import Point2, Point3, SfmData, SfmTrack
from gtsam.utils.test_case import GtsamTestCase
@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase):
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = gtsam.SfmData()
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = gtsam.SfmTrack()
self.tracks = SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4,5
i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = gtsam.Point2(12.6, 82)
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82)
uv_i2 = Point2(24.88, 82)
# add measurements to the track
self.tracks.add_measurement(i1, uv_i1)
self.tracks.add_measurement(i2, uv_i2)
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.number_measurements(), 2)
self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(
gtsam.Point3(0.,0.,0.),
Point3(0., 0., 0.),
self.tracks.point3()
)
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3,5,6
uv_i1 = gtsam.Point2(21.23, 45.64)
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = gtsam.Point2(45.7, 45.64)
uv_i3 = gtsam.Point2(68.35, 45.64)
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = gtsam.Point3(1.0, 6.0, 2.0)
track2 = gtsam.SfmTrack(pt)
track2.add_measurement(i1, uv_i1)
track2.add_measurement(i2, uv_i2)
track2.add_measurement(i3, uv_i3)
self.data.add_track(self.tracks)
self.data.add_track(track2)
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.number_tracks(), 2)
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
def test_Balbianello(self):
""" Check that we can successfully read a bundler file and create a
factor graph from it
"""
# The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
self.assertEqual(5, sfm_data.numberCameras())
self.assertEqual(544, sfm_data.numberTracks())
track0 = sfm_data.track(0)
self.assertEqual(3, track0.numberMeasurements())
# Check projection of a given point
self.assertEqual(0, track0.measurement(0)[0])
camera0 = sfm_data.camera(0)
expected = camera0.project(track0.point3())
actual = track0.measurement(0)[1]
self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model)
self.assertEqual(1419, graph.size()) # regression
# Get initial estimate
values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
self.assertEqual(549, values.size()) # regression
if __name__ == '__main__':
unittest.main()

4
python/gtsam/tests/test_pickle.py
View File

@ -37,8 +37,8 @@ class TestPickle(GtsamTestCase):
def test_sfmTrack_roundtrip(self):
obj = SfmTrack(Point3(1, 1, 0))
obj.add_measurement(0, Point2(-1, 5))
obj.add_measurement(1, Point2(6, 2))
obj.addMeasurement(0, Point2(-1, 5))
obj.addMeasurement(1, Point2(6, 2))
self.assertEqualityOnPickleRoundtrip(obj)
def test_unit3_roundtrip(self):

11
python/gtsam/utils/visual_data_generator.py
View File

@ -1,12 +1,12 @@
from __future__ import print_function
from typing import Tuple
import math
import numpy as np
from math import pi
from typing import Tuple
import gtsam
from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
import numpy as np
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
class Options:
@ -36,7 +36,7 @@ class GroundTruth:
self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints
def print_(self, s="") -> None:
def print(self, s: str = "") -> None:
print(s)
print("K = ", self.K)
print("Cameras: ", len(self.cameras))
@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),

2
python/gtsam/utils/visual_isam.py
View File

@ -17,7 +17,7 @@ def initialize(data, truth, options):
# Initialize iSAM
params = gtsam.ISAM2Params()
if options.alwaysRelinearize:
params.setRelinearizeSkip(1)
params.relinearizeSkip = 1
isam = gtsam.ISAM2(params=params)
# Add constraints/priors

40
tests/testGaussianBayesTreeB.cpp
View File

@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
C4 x7 : x6
************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
TEST(GaussianBayesTree, balanced_smoother_marginals) {
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4);
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
VectorValues actualSolution = bayesTree.optimize();
VectorValues expectedSolution = VectorValues::Zero(actualSolution);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
EXPECT(assert_equal(expectedSolution, actualSolution, tol));
LONGS_EQUAL(4, (long)bayesTree.size());
LONGS_EQUAL(4, bayesTree.size());
double tol=1e-5;
double tol = 1e-5;
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
Matrix actualCovarianceX1 = m->information().inverse();
EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
EXPECT(assert_equal(expected2,actual2,tol));
Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
// Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
EXPECT(assert_equal(expected3,actual3,tol));
Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
// Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
EXPECT(assert_equal(expected4,actual4,tol));
Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
// Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
EXPECT(assert_equal(expected7,actual7,tol));
Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
}
/* ************************************************************************* */

7
tests/testGeneralSFMFactorB.cpp
View File

@ -15,6 +15,7 @@
* @brief test general SFM class, with nonlinear optimization and BAL files
*/
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Point3.h>
@ -42,14 +43,12 @@ using symbol_shorthand::P;
/* ************************************************************************* */
TEST(PinholeCamera, BAL) {
string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
SfmData db = SfmData::FromBalFile(filename);
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements)
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
}

5
tests/testTranslationRecovery.cpp
View File

@ -18,6 +18,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
using namespace std;
@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses,
// sets up an optimization problem for the three unknown translations.
TEST(TranslationRecovery, BAL) {
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
SfmData db = SfmData::FromBalFile(filename);
// Get camera poses, as Values
size_t j = 0;

2
timing/timeSFMBAL.cpp
View File

@ -36,7 +36,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;

10
timing/timeSFMBAL.h
View File

@ -16,7 +16,7 @@
* @date July 5, 2015
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) {
filename = argv[argc - 1];
else
filename = findExampleDataFile("dubrovnik-16-22106-pre");
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
return db;
return SfmData::FromBalFile(filename);
}
// Create ordering and optimize
@ -73,8 +71,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
if (gUseSchur) {
// Create Schur-complement ordering
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++) {
for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
for (size_t i = 0; i < db.numberCameras(); i++) {
ordering.push_back(C(i));
if (separateCalibration) ordering.push_back(K(i));
}

4
timing/timeSFMBALautodiff.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
// Build graph
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;
@ -59,7 +59,7 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera& camera: db.cameras) {
// readBAL converts to GTSAM format, so we need to convert back !
// SfmData::FromBalFile converts to GTSAM format, so we need to convert back !
Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration();

2
timing/timeSFMBALcamTnav.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;

2
timing/timeSFMBALnavTcam.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
Point3_ nav_point_(P(j));
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;

2
timing/timeSFMBALsmart.cpp
View File

@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
// Add smart factors to graph
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
for (const SfmMeasurement& m : db.tracks[j].measurements) {
size_t i = m.first;

21
wrap/gtwrap/matlab_wrapper/mixins.py
View File

@ -26,25 +26,30 @@ class CheckMixin:
return True
return False
def can_be_pointer(self, arg_type: parser.Type):
"""
Determine if the `arg_type` can have a pointer to it.
E.g. `Pose3` can have `Pose3*` but
`Matrix` should not have `Matrix*`.
"""
return (arg_type.typename.name not in self.not_ptr_type
and arg_type.typename.name not in self.ignore_namespace
and arg_type.typename.name != 'string')
def is_shared_ptr(self, arg_type: parser.Type):
"""
Determine if the `interface_parser.Type` should be treated as a
shared pointer in the wrapper.
"""
return arg_type.is_shared_ptr or (
arg_type.typename.name not in self.not_ptr_type
and arg_type.typename.name not in self.ignore_namespace
and arg_type.typename.name != 'string')
return arg_type.is_shared_ptr
def is_ptr(self, arg_type: parser.Type):
"""
Determine if the `interface_parser.Type` should be treated as a
raw pointer in the wrapper.
"""
return arg_type.is_ptr or (
arg_type.typename.name not in self.not_ptr_type
and arg_type.typename.name not in self.ignore_namespace
and arg_type.typename.name != 'string')
return arg_type.is_ptr
def is_ref(self, arg_type: parser.Type):
"""

92
wrap/gtwrap/matlab_wrapper/wrapper.py
View File

@ -147,11 +147,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
"""
def args_copy(args):
return ArgumentList([copy.copy(arg) for arg in args.list()])
def method_copy(method):
method2 = copy.copy(method)
method2.args = args_copy(method.args)
method2.args.backup = method.args.backup
return method2
if save_backup:
method.args.backup = args_copy(method.args)
method = method_copy(method)
@ -162,7 +164,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
method.args.list().remove(arg)
return [
methodWithArg,
*MatlabWrapper._expand_default_arguments(method, save_backup=False)
*MatlabWrapper._expand_default_arguments(method,
save_backup=False)
]
break
assert all(arg.default is None for arg in method.args.list()), \
@ -180,9 +183,12 @@ class MatlabWrapper(CheckMixin, FormatMixin):
if method_index is None:
method_map[method.name] = len(method_out)
method_out.append(MatlabWrapper._expand_default_arguments(method))
method_out.append(
MatlabWrapper._expand_default_arguments(method))
else:
method_out[method_index] += MatlabWrapper._expand_default_arguments(method)
method_out[
method_index] += MatlabWrapper._expand_default_arguments(
method)
return method_out
@ -337,43 +343,42 @@ class MatlabWrapper(CheckMixin, FormatMixin):
body_args = ''
for arg in args.list():
ctype_camel = self._format_type_name(arg.ctype.typename,
separator='')
ctype_sep = self._format_type_name(arg.ctype.typename)
if self.is_ref(arg.ctype): # and not constructor:
ctype_camel = self._format_type_name(arg.ctype.typename,
separator='')
body_args += textwrap.indent(textwrap.dedent('''\
{ctype}& {name} = *unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");
'''.format(ctype=self._format_type_name(arg.ctype.typename),
ctype_camel=ctype_camel,
name=arg.name,
id=arg_id)),
prefix=' ')
arg_type = "{ctype}&".format(ctype=ctype_sep)
unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format(
ctype=ctype_sep, ctype_camel=ctype_camel, id=arg_id)
elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \
elif self.is_ptr(arg.ctype) and \
arg.ctype.typename.name not in self.ignore_namespace:
if arg.ctype.is_shared_ptr:
call_type = arg.ctype.is_shared_ptr
else:
call_type = arg.ctype.is_ptr
body_args += textwrap.indent(textwrap.dedent('''\
{std_boost}::shared_ptr<{ctype_sep}> {name} = unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");
'''.format(std_boost='boost' if constructor else 'boost',
ctype_sep=self._format_type_name(
arg.ctype.typename),
ctype=self._format_type_name(arg.ctype.typename,
separator=''),
name=arg.name,
id=arg_id)),
prefix=' ')
arg_type = "{ctype_sep}*".format(ctype_sep=ctype_sep)
unwrap = 'unwrap_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
elif (self.is_shared_ptr(arg.ctype) or self.can_be_pointer(arg.ctype)) and \
arg.ctype.typename.name not in self.ignore_namespace:
call_type = arg.ctype.is_shared_ptr
arg_type = "{std_boost}::shared_ptr<{ctype_sep}>".format(
std_boost='boost' if constructor else 'boost',
ctype_sep=ctype_sep)
unwrap = 'unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
else:
body_args += textwrap.indent(textwrap.dedent('''\
{ctype} {name} = unwrap< {ctype} >(in[{id}]);
'''.format(ctype=arg.ctype.typename.name,
name=arg.name,
id=arg_id)),
prefix=' ')
arg_type = "{ctype}".format(ctype=arg.ctype.typename.name)
unwrap = 'unwrap< {ctype} >(in[{id}]);'.format(
ctype=arg.ctype.typename.name, id=arg_id)
body_args += textwrap.indent(textwrap.dedent('''\
{arg_type} {name} = {unwrap}
'''.format(arg_type=arg_type, name=arg.name,
unwrap=unwrap)),
prefix=' ')
arg_id += 1
params = ''
@ -383,12 +388,14 @@ class MatlabWrapper(CheckMixin, FormatMixin):
if params != '':
params += ','
if (arg.default is not None) and (arg.name not in explicit_arg_names):
if (arg.default is not None) and (arg.name
not in explicit_arg_names):
params += arg.default
continue
if (not self.is_ref(arg.ctype)) and (self.is_shared_ptr(arg.ctype)) and (self.is_ptr(
arg.ctype)) and (arg.ctype.typename.name not in self.ignore_namespace):
if not self.is_ref(arg.ctype) and (self.is_shared_ptr(arg.ctype) or \
self.is_ptr(arg.ctype) or self.can_be_pointer(arg.ctype))and \
arg.ctype.typename.name not in self.ignore_namespace:
if arg.ctype.is_shared_ptr:
call_type = arg.ctype.is_shared_ptr
else:
@ -601,7 +608,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
if not isinstance(ctors, Iterable):
ctors = [ctors]
ctors = sum((MatlabWrapper._expand_default_arguments(ctor) for ctor in ctors), [])
ctors = sum((MatlabWrapper._expand_default_arguments(ctor)
for ctor in ctors), [])
methods_wrap = textwrap.indent(textwrap.dedent("""\
methods
@ -885,10 +893,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
wrapper=self._wrapper_name(),
id=self._update_wrapper_id(
(namespace_name, instantiated_class,
static_overload.name, static_overload)),
static_overload.name, static_overload)),
class_name=instantiated_class.name,
end_statement=end_statement),
prefix=' ')
prefix=' ')
# If the arguments don't match any of the checks above,
# throw an error with the class and method name.
@ -1079,7 +1087,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
pair_value = 'first' if func_id == 0 else 'second'
new_line = '\n' if func_id == 0 else ''
if self.is_shared_ptr(return_type) or self.is_ptr(return_type):
if self.is_shared_ptr(return_type) or self.is_ptr(return_type) or \
self.can_be_pointer(return_type):
shared_obj = 'pairResult.' + pair_value
if not (return_type.is_shared_ptr or return_type.is_ptr):
@ -1145,7 +1154,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
if return_1_name != 'void':
if return_count == 1:
if self.is_shared_ptr(return_1) or self.is_ptr(return_1):
if self.is_shared_ptr(return_1) or self.is_ptr(return_1) or \
self.can_be_pointer(return_1):
sep_method_name = partial(self._format_type_name,
return_1.typename,
include_namespace=True)

8
wrap/matlab.h
View File

@ -477,6 +477,14 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
return *spp;
}
template <typename Class>
Class* unwrap_ptr(const mxArray* obj, const string& propertyName) {
mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
Class* x = reinterpret_cast<Class*> (mxGetData(mxh));
return x;
}
//// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
//template <>
//Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {

6
wrap/tests/expected/matlab/ForwardKinematicsFactor.m
View File

@ -11,9 +11,9 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
if nargin == 2
my_ptr = varargin{2};
else
my_ptr = inheritance_wrapper(36, varargin{2});
my_ptr = inheritance_wrapper(52, varargin{2});
end
base_ptr = inheritance_wrapper(35, my_ptr);
base_ptr = inheritance_wrapper(51, my_ptr);
else
error('Arguments do not match any overload of ForwardKinematicsFactor constructor');
end
@ -22,7 +22,7 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
end
function delete(obj)
inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor);
inheritance_wrapper(53, obj.ptr_ForwardKinematicsFactor);
end
function display(obj), obj.print(''); end

2
wrap/tests/expected/matlab/functions_wrapper.cpp
View File

@ -86,7 +86,7 @@ void load2D_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("load2D",nargout,nargin,2);
string filename = unwrap< string >(in[0]);
boost::shared_ptr<gtsam::noiseModel::Diagonal> model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
gtsam::noiseModel::Diagonal* model = unwrap_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
auto pairResult = load2D(filename,model);
out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);

4
wrap/tests/expected/matlab/geometry_wrapper.cpp
View File

@ -151,7 +151,7 @@ void gtsamPoint2_argChar_7(int nargout, mxArray *out[], int nargin, const mxArra
{
checkArguments("argChar",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
char* a = unwrap_ptr< char >(in[1], "ptr_char");
obj->argChar(a);
}
@ -175,7 +175,7 @@ void gtsamPoint2_argChar_10(int nargout, mxArray *out[], int nargin, const mxArr
{
checkArguments("argChar",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
char* a = unwrap_ptr< char >(in[1], "ptr_char");
obj->argChar(a);
}

220
wrap/tests/expected/matlab/inheritance_wrapper.cpp
View File

@ -9,6 +9,7 @@
typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
typedef MyTemplate<A> MyTemplateA;
typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
static Collector_MyBase collector_MyBase;
@ -16,6 +17,8 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint
static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
typedef std::set<boost::shared_ptr<MyTemplateA>*> Collector_MyTemplateA;
static Collector_MyTemplateA collector_MyTemplateA;
typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
@ -44,6 +47,12 @@ void _deleteAllObjects()
collector_MyTemplateMatrix.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_MyTemplateA::iterator iter = collector_MyTemplateA.begin();
iter != collector_MyTemplateA.end(); ) {
delete *iter;
collector_MyTemplateA.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin();
iter != collector_ForwardKinematicsFactor.end(); ) {
delete *iter;
@ -67,6 +76,7 @@ void _inheritance_RTTIRegister() {
types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
types.insert(std::make_pair(typeid(MyTemplateA).name(), "MyTemplateA"));
types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
@ -462,7 +472,157 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx
out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
}
void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void MyTemplateA_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MyTemplate<A>> Shared;
Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
collector_MyTemplateA.insert(self);
typedef boost::shared_ptr<MyBase> SharedBase;
out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
}
void MyTemplateA_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MyTemplate<A>> Shared;
boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<A>>(*asVoid));
*reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
}
void MyTemplateA_constructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MyTemplate<A>> Shared;
Shared *self = new Shared(new MyTemplate<A>());
collector_MyTemplateA.insert(self);
out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
typedef boost::shared_ptr<MyBase> SharedBase;
out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
}
void MyTemplateA_deconstructor_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<MyTemplate<A>> Shared;
checkArguments("delete_MyTemplateA",nargout,nargin,1);
Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
Collector_MyTemplateA::iterator item;
item = collector_MyTemplateA.find(self);
if(item != collector_MyTemplateA.end()) {
collector_MyTemplateA.erase(item);
}
delete self;
}
void MyTemplateA_accept_T_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("accept_T",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
A& value = *unwrap_shared_ptr< A >(in[1], "ptr_A");
obj->accept_T(value);
}
void MyTemplateA_accept_Tptr_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("accept_Tptr",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
obj->accept_Tptr(value);
}
void MyTemplateA_create_MixedPtrs_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("create_MixedPtrs",nargout,nargin-1,0);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
auto pairResult = obj->create_MixedPtrs();
out[0] = wrap_shared_ptr(boost::make_shared<A>(pairResult.first),"A", false);
out[1] = wrap_shared_ptr(pairResult.second,"A", false);
}
void MyTemplateA_create_ptrs_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("create_ptrs",nargout,nargin-1,0);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
auto pairResult = obj->create_ptrs();
out[0] = wrap_shared_ptr(pairResult.first,"A", false);
out[1] = wrap_shared_ptr(pairResult.second,"A", false);
}
void MyTemplateA_return_T_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("return_T",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
A* value = unwrap_ptr< A >(in[1], "ptr_A");
out[0] = wrap_shared_ptr(boost::make_shared<A>(obj->return_T(value)),"A", false);
}
void MyTemplateA_return_Tptr_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("return_Tptr",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
out[0] = wrap_shared_ptr(obj->return_Tptr(value),"A", false);
}
void MyTemplateA_return_ptrs_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("return_ptrs",nargout,nargin-1,2);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
boost::shared_ptr<A> p1 = unwrap_shared_ptr< A >(in[1], "ptr_A");
boost::shared_ptr<A> p2 = unwrap_shared_ptr< A >(in[2], "ptr_A");
auto pairResult = obj->return_ptrs(p1,p2);
out[0] = wrap_shared_ptr(pairResult.first,"A", false);
out[1] = wrap_shared_ptr(pairResult.second,"A", false);
}
void MyTemplateA_templatedMethod_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
Matrix t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
}
void MyTemplateA_templatedMethod_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
Point2 t = unwrap< Point2 >(in[1]);
out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
}
void MyTemplateA_templatedMethod_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
Point3 t = unwrap< Point3 >(in[1]);
out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
}
void MyTemplateA_templatedMethod_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("templatedMethodVector",nargout,nargin-1,1);
auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
Vector t = unwrap< Vector >(in[1]);
out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
}
void MyTemplateA_Level_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("MyTemplate<A>.Level",nargout,nargin,1);
A& K = *unwrap_shared_ptr< A >(in[0], "ptr_A");
out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<A>>(MyTemplate<A>::Level(K)),"MyTemplateA", false);
}
void ForwardKinematicsFactor_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
@ -475,7 +635,7 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray
*reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
}
void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
void ForwardKinematicsFactor_upcastFromVoid_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
@ -484,7 +644,7 @@ void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int
*reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
}
void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void ForwardKinematicsFactor_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
checkArguments("delete_ForwardKinematicsFactor",nargout,nargin,1);
@ -615,13 +775,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
break;
case 35:
ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
MyTemplateA_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
break;
case 36:
ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1);
MyTemplateA_upcastFromVoid_36(nargout, out, nargin-1, in+1);
break;
case 37:
ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1);
MyTemplateA_constructor_37(nargout, out, nargin-1, in+1);
break;
case 38:
MyTemplateA_deconstructor_38(nargout, out, nargin-1, in+1);
break;
case 39:
MyTemplateA_accept_T_39(nargout, out, nargin-1, in+1);
break;
case 40:
MyTemplateA_accept_Tptr_40(nargout, out, nargin-1, in+1);
break;
case 41:
MyTemplateA_create_MixedPtrs_41(nargout, out, nargin-1, in+1);
break;
case 42:
MyTemplateA_create_ptrs_42(nargout, out, nargin-1, in+1);
break;
case 43:
MyTemplateA_return_T_43(nargout, out, nargin-1, in+1);
break;
case 44:
MyTemplateA_return_Tptr_44(nargout, out, nargin-1, in+1);
break;
case 45:
MyTemplateA_return_ptrs_45(nargout, out, nargin-1, in+1);
break;
case 46:
MyTemplateA_templatedMethod_46(nargout, out, nargin-1, in+1);
break;
case 47:
MyTemplateA_templatedMethod_47(nargout, out, nargin-1, in+1);
break;
case 48:
MyTemplateA_templatedMethod_48(nargout, out, nargin-1, in+1);
break;
case 49:
MyTemplateA_templatedMethod_49(nargout, out, nargin-1, in+1);
break;
case 50:
MyTemplateA_Level_50(nargout, out, nargin-1, in+1);
break;
case 51:
ForwardKinematicsFactor_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
break;
case 52:
ForwardKinematicsFactor_upcastFromVoid_52(nargout, out, nargin-1, in+1);
break;
case 53:
ForwardKinematicsFactor_deconstructor_53(nargout, out, nargin-1, in+1);
break;
}
} catch(const std::exception& e) {

15
wrap/tests/expected/python/inheritance_pybind.cpp
View File

@ -54,6 +54,21 @@ PYBIND11_MODULE(inheritance_py, m_) {
.def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self, const std::shared_ptr<gtsam::Matrix> p1, const std::shared_ptr<gtsam::Matrix> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
.def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
py::class_<MyTemplate<A>, MyBase, std::shared_ptr<MyTemplate<A>>>(m_, "MyTemplateA")
.def(py::init<>())
.def("templatedMethodPoint2",[](MyTemplate<A>* self, const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t"))
.def("templatedMethodPoint3",[](MyTemplate<A>* self, const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t"))
.def("templatedMethodVector",[](MyTemplate<A>* self, const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t"))
.def("templatedMethodMatrix",[](MyTemplate<A>* self, const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t"))
.def("accept_T",[](MyTemplate<A>* self, const A& value){ self->accept_T(value);}, py::arg("value"))
.def("accept_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){ self->accept_Tptr(value);}, py::arg("value"))
.def("return_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){return self->return_Tptr(value);}, py::arg("value"))
.def("return_T",[](MyTemplate<A>* self, A* value){return self->return_T(value);}, py::arg("value"))
.def("create_ptrs",[](MyTemplate<A>* self){return self->create_ptrs();})
.def("create_MixedPtrs",[](MyTemplate<A>* self){return self->create_MixedPtrs();})
.def("return_ptrs",[](MyTemplate<A>* self, std::shared_ptr<A> p1, std::shared_ptr<A> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
.def_static("Level",[](const A& K){return MyTemplate<A>::Level(K);}, py::arg("K"));
py::class_<ForwardKinematicsFactor, gtsam::BetweenFactor<gtsam::Pose3>, std::shared_ptr<ForwardKinematicsFactor>>(m_, "ForwardKinematicsFactor");

Some files were not shown because too many files have changed in this diff Show More