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

Merge branch 'hybrid/tests' into hybrid/verification

release/4.3a0
Varun Agrawal 2022-12-24 20:11:26 +05:30
parent aa86af2d77 153c12e18a
commit 6b834db828
107 changed files with 1636 additions and 904 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CameraResectioning.cpp
View File

@ -30,8 +30,8 @@ using symbol_shorthand::X;
* Unary factor on the unknown pose, resulting from meauring the projection of
* a known 3D point in the image
*/
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
typedef NoiseModelFactor1<Pose3> Base;
class ResectioningFactor: public NoiseModelFactorN<Pose3> {
typedef NoiseModelFactorN<Pose3> Base;
Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
Point3 P_; ///< 3D point on the calibration rig

8
examples/LocalizationExample.cpp
View File

@ -62,10 +62,10 @@ using namespace gtsam;
//
// The factor will be a unary factor, affect only a single system variable. It will
// also use a standard Gaussian noise model. Hence, we will derive our new factor from
// the NoiseModelFactor1.
// the NoiseModelFactorN.
#include <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> {
class UnaryFactor: public NoiseModelFactorN<Pose2> {
// The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles
double mx_, my_;
@ -76,11 +76,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The constructor requires the variable key, the (X, Y) measurement value, and the noise model
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
NoiseModelFactorN<Pose2>(model, j), mx_(x), my_(y) {}
~UnaryFactor() override {}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden.
// Using the NoiseModelFactorN base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.

8
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -71,11 +71,11 @@ int main(const int argc, const char *argv[]) {
if (pose3Between){
Key key1, key2;
if(add){
key1 = pose3Between->key1() + firstKey;
key2 = pose3Between->key2() + firstKey;
key1 = pose3Between->key<1>() + firstKey;
key2 = pose3Between->key<2>() + firstKey;
}else{
key1 = pose3Between->key1() - firstKey;
key2 = pose3Between->key2() - firstKey;
key1 = pose3Between->key<1>() - firstKey;
key2 = pose3Between->key<2>() - firstKey;
}
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));

28
examples/SolverComparer.cpp
View File

@ -69,8 +69,8 @@ namespace br = boost::range;
typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef NoiseModelFactorN<Pose> NM1;
typedef NoiseModelFactorN<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
@ -261,7 +261,7 @@ void runIncremental()
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = factor->key1(), key2 = factor->key2();
Key key1 = factor->key<1>(), key2 = factor->key<2>();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
@ -313,11 +313,11 @@ void runIncremental()
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{
// Stop collecting measurements that are for future steps
if(factor->key1() > step || factor->key2() > step)
if(factor->key<1>() > step || factor->key<2>() > step)
break;
// Require that one of the nodes is the current one
if(factor->key1() != step && factor->key2() != step)
if(factor->key<1>() != step && factor->key<2>() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor
@ -325,22 +325,22 @@ void runIncremental()
const auto& measured = factor->measured();
// Initialize the new variable
if(factor->key1() > factor->key2()) {
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
if(factor->key<1>() > factor->key<2>()) {
if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(factor->key1(), measured.inverse());
newVariables.insert(factor->key<1>(), measured.inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
newVariables.insert(factor->key1(), prevPose * measured.inverse());
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
newVariables.insert(factor->key<1>(), prevPose * measured.inverse());
}
}
} else {
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(factor->key2(), measured);
newVariables.insert(factor->key<2>(), measured);
else {
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
newVariables.insert(factor->key2(), prevPose * measured);
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
newVariables.insert(factor->key<2>(), prevPose * measured);
}
}
}

2
gtsam/base/Testable.h
View File

@ -23,7 +23,7 @@
* void print(const std::string& name) const = 0;
*
* equality up to tolerance
* tricky to implement, see NoiseModelFactor1 for an example
* tricky to implement, see PriorFactor for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*

39
gtsam/base/types.h
View File

@ -46,18 +46,49 @@
#include <omp.h>
#endif
/* Define macros for ignoring compiler warnings.
* Usage Example:
* ```
* CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
* GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
* MSVC_DIAGNOSTIC_PUSH_IGNORE(4996)
* // ... code you want to suppress deprecation warnings for ...
* DIAGNOSTIC_POP()
* ```
*/
#define DO_PRAGMA(x) _Pragma (#x)
#ifdef __clang__
# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \
_Pragma("clang diagnostic push") \
_Pragma("clang diagnostic ignored \"" diag "\"")
DO_PRAGMA(clang diagnostic ignored diag)
#else
# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
#endif
#ifdef __clang__
# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
#ifdef __GNUC__
# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \
_Pragma("GCC diagnostic push") \
DO_PRAGMA(GCC diagnostic ignored diag)
#else
# define CLANG_DIAGNOSTIC_POP()
# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag)
#endif
#ifdef _MSC_VER
# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \
_Pragma("warning ( push )") \
DO_PRAGMA(warning ( disable : code ))
#else
# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code)
#endif
#if defined(__clang__)
# define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
#elif defined(__GNUC__)
# define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop")
#elif defined(_MSC_VER)
# define DIAGNOSTIC_POP() _Pragma("warning ( pop )")
#else
# define DIAGNOSTIC_POP()
#endif
namespace gtsam {

39
gtsam/base/utilities.h
View File

@ -27,3 +27,42 @@ private:
};
}
// boost::index_sequence was introduced in 1.66, so we'll manually define an
// implementation if user has 1.65. boost::index_sequence is used to get array
// indices that align with a parameter pack.
#include <boost/version.hpp>
#if BOOST_VERSION >= 106600
#include <boost/mp11/integer_sequence.hpp>
#else
namespace boost {
namespace mp11 {
// Adapted from https://stackoverflow.com/a/32223343/9151520
template <size_t... Ints>
struct index_sequence {
using type = index_sequence;
using value_type = size_t;
static constexpr std::size_t size() noexcept { return sizeof...(Ints); }
};
namespace detail {
template <class Sequence1, class Sequence2>
struct _merge_and_renumber;
template <size_t... I1, size_t... I2>
struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...> >
: index_sequence<I1..., (sizeof...(I1) + I2)...> {};
} // namespace detail
template <size_t N>
struct make_index_sequence
: detail::_merge_and_renumber<
typename make_index_sequence<N / 2>::type,
typename make_index_sequence<N - N / 2>::type> {};
template <>
struct make_index_sequence<0> : index_sequence<> {};
template <>
struct make_index_sequence<1> : index_sequence<0> {};
template <class... T>
using index_sequence_for = make_index_sequence<sizeof...(T)>;
} // namespace mp11
} // namespace boost
#endif

20
gtsam/hybrid/GaussianMixture.cpp
View File

@ -85,8 +85,8 @@ size_t GaussianMixture::nrComponents() const {
/* *******************************************************************************/
GaussianConditional::shared_ptr GaussianMixture::operator()(
const DiscreteValues &discreteVals) const {
auto &ptr = conditionals_(discreteVals);
const DiscreteValues &discreteValues) const {
auto &ptr = conditionals_(discreteValues);
if (!ptr) return nullptr;
auto conditional = boost::dynamic_pointer_cast<GaussianConditional>(ptr);
if (conditional)
@ -209,14 +209,15 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixture::error(
const VectorValues &continuousVals) const {
// functor to convert from GaussianConditional to double error value.
const VectorValues &continuousValues) const {
// functor to calculate to double error value from GaussianConditional.
auto errorFunc =
[continuousVals](const GaussianConditional::shared_ptr &conditional) {
[continuousValues](const GaussianConditional::shared_ptr &conditional) {
if (conditional) {
return conditional->error(continuousVals);
return conditional->error(continuousValues);
} else {
// return arbitrarily large error
// Return arbitrarily large error if conditional is null
// Conditional is null if it is pruned out.
return 1e50;
}
};
@ -225,10 +226,11 @@ AlgebraicDecisionTree<Key> GaussianMixture::error(
}
/* *******************************************************************************/
double GaussianMixture::error(const VectorValues &continuousVals,
double GaussianMixture::error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const {
// Directly index to get the conditional, no need to build the whole tree.
auto conditional = conditionals_(discreteValues);
return conditional->error(continuousVals);
return conditional->error(continuousValues);
}
} // namespace gtsam

14
gtsam/hybrid/GaussianMixture.h
View File

@ -122,7 +122,7 @@ class GTSAM_EXPORT GaussianMixture
/// @{
GaussianConditional::shared_ptr operator()(
const DiscreteValues &discreteVals) const;
const DiscreteValues &discreteValues) const;
/// Returns the total number of continuous components
size_t nrComponents() const;
@ -147,21 +147,21 @@ class GTSAM_EXPORT GaussianMixture
/**
* @brief Compute error of the GaussianMixture as a tree.
*
* @param continuousVals The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
* @param continuousValues The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
* as the conditionals, and leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param continuousVals The continuous values at which to compute the error.
* @param continuousValues Continuous values at which to compute the error.
* @param discreteValues The discrete assignment for a specific mode sequence.
* @return double
*/
double error(const VectorValues &continuousVals,
double error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const;
/**

14
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -98,21 +98,23 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixtureFactor::error(
const VectorValues &continuousVals) const {
const VectorValues &continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) {
return factor->error(continuousVals);
};
auto errorFunc =
[continuousValues](const GaussianFactor::shared_ptr &factor) {
return factor->error(continuousValues);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
/* *******************************************************************************/
double GaussianMixtureFactor::error(
const VectorValues &continuousVals,
const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const {
// Directly index to get the conditional, no need to build the whole tree.
auto factor = factors_(discreteValues);
return factor->error(continuousVals);
return factor->error(continuousValues);
}
} // namespace gtsam

12
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -133,21 +133,21 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
/**
* @brief Compute error of the GaussianMixtureFactor as a tree.
*
* @param continuousVals The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
* @param continuousValues The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
* as the factors involved, and leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param continuousVals The continuous values at which to compute the error.
* @param continuousValues Continuous values at which to compute the error.
* @param discreteValues The discrete assignment for a specific mode sequence.
* @return double
*/
double error(const VectorValues &continuousVals,
double error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const;
/// Add MixtureFactor to a Sum, syntactic sugar.

110
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -8,7 +8,7 @@
/**
* @file HybridBayesNet.cpp
* @brief A bayes net of Gaussian Conditionals indexed by discrete keys.
* @brief A Bayes net of Gaussian Conditionals indexed by discrete keys.
* @author Fan Jiang
* @author Varun Agrawal
* @author Shangjie Xue
@ -20,18 +20,20 @@
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridValues.h>
// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);
namespace gtsam {
/* ************************************************************************* */
DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
AlgebraicDecisionTree<Key> decisionTree;
// The canonical decision tree factor which will get the discrete conditionals
// added to it.
// The canonical decision tree factor which will get
// the discrete conditionals added to it.
DecisionTreeFactor dtFactor;
for (size_t i = 0; i < this->size(); i++) {
HybridConditional::shared_ptr conditional = this->at(i);
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
// Convert to a DecisionTreeFactor and add it to the main factor.
DecisionTreeFactor f(*conditional->asDiscreteConditional());
@ -53,7 +55,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
const DecisionTreeFactor &decisionTree,
const HybridConditional &conditional) {
// Get the discrete keys as sets for the decision tree
// and the gaussian mixture.
// and the Gaussian mixture.
auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
auto conditionalKeySet = DiscreteKeysAsSet(conditional.discreteKeys());
@ -62,7 +64,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
double probability) -> double {
// typecast so we can use this to get probability value
DiscreteValues values(choices);
// Case where the gaussian mixture has the same
// Case where the Gaussian mixture has the same
// discrete keys as the decision tree.
if (conditionalKeySet == decisionTreeKeySet) {
if (decisionTree(values) == 0) {
@ -101,6 +103,7 @@ void HybridBayesNet::updateDiscreteConditionals(
const DecisionTreeFactor::shared_ptr &prunedDecisionTree) {
KeyVector prunedTreeKeys = prunedDecisionTree->keys();
// Loop with index since we need it later.
for (size_t i = 0; i < this->size(); i++) {
HybridConditional::shared_ptr conditional = this->at(i);
if (conditional->isDiscrete()) {
@ -153,7 +156,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
if (conditional->isHybrid()) {
GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture();
// Make a copy of the gaussian mixture and prune it!
// Make a copy of the Gaussian mixture and prune it!
auto prunedGaussianMixture =
boost::make_shared<GaussianMixture>(*gaussianMixture);
prunedGaussianMixture->prune(*decisionTree);
@ -173,35 +176,35 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
/* ************************************************************************* */
GaussianMixture::shared_ptr HybridBayesNet::atMixture(size_t i) const {
return factors_.at(i)->asMixture();
return at(i)->asMixture();
}
/* ************************************************************************* */
GaussianConditional::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
return factors_.at(i)->asGaussian();
return at(i)->asGaussian();
}
/* ************************************************************************* */
DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
return factors_.at(i)->asDiscreteConditional();
return at(i)->asDiscreteConditional();
}
/* ************************************************************************* */
GaussianBayesNet HybridBayesNet::choose(
const DiscreteValues &assignment) const {
GaussianBayesNet gbn;
for (size_t idx = 0; idx < size(); idx++) {
if (factors_.at(idx)->isHybrid()) {
// If factor is hybrid, select based on assignment.
GaussianMixture gm = *this->atMixture(idx);
for (auto &&conditional : *this) {
if (conditional->isHybrid()) {
// If conditional is hybrid, select based on assignment.
GaussianMixture gm = *conditional->asMixture();
gbn.push_back(gm(assignment));
} else if (factors_.at(idx)->isContinuous()) {
// If continuous only, add gaussian conditional.
gbn.push_back((this->atGaussian(idx)));
} else if (conditional->isContinuous()) {
// If continuous only, add Gaussian conditional.
gbn.push_back((conditional->asGaussian()));
} else if (factors_.at(idx)->isDiscrete()) {
// If factor at `idx` is discrete-only, we simply continue.
} else if (conditional->isDiscrete()) {
// If conditional is discrete-only, we simply continue.
continue;
}
}
@ -213,7 +216,7 @@ GaussianBayesNet HybridBayesNet::choose(
HybridValues HybridBayesNet::optimize() const {
// Solve for the MPE
DiscreteBayesNet discrete_bn;
for (auto &conditional : factors_) {
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
discrete_bn.push_back(conditional->asDiscreteConditional());
}
@ -238,6 +241,41 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
return gbn.optimize();
}
/* ************************************************************************* */
HybridValues HybridBayesNet::sample(const HybridValues &given,
std::mt19937_64 *rng) const {
DiscreteBayesNet dbn;
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
// If conditional is discrete-only, we add to the discrete Bayes net.
dbn.push_back(conditional->asDiscreteConditional());
}
}
// Sample a discrete assignment.
const DiscreteValues assignment = dbn.sample(given.discrete());
// Select the continuous Bayes net corresponding to the assignment.
GaussianBayesNet gbn = choose(assignment);
// Sample from the Gaussian Bayes net.
VectorValues sample = gbn.sample(given.continuous(), rng);
return {assignment, sample};
}
/* ************************************************************************* */
HybridValues HybridBayesNet::sample(std::mt19937_64 *rng) const {
HybridValues given;
return sample(given, rng);
}
/* ************************************************************************* */
HybridValues HybridBayesNet::sample(const HybridValues &given) const {
return sample(given, &kRandomNumberGenerator);
}
/* ************************************************************************* */
HybridValues HybridBayesNet::sample() const {
return sample(&kRandomNumberGenerator);
}
/* ************************************************************************* */
double HybridBayesNet::error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const {
@ -248,30 +286,28 @@ double HybridBayesNet::error(const VectorValues &continuousValues,
/* ************************************************************************* */
AlgebraicDecisionTree<Key> HybridBayesNet::error(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree;
AlgebraicDecisionTree<Key> error_tree(0.0);
for (size_t idx = 0; idx < size(); idx++) {
AlgebraicDecisionTree<Key> conditional_error;
if (factors_.at(idx)->isHybrid()) {
// If factor is hybrid, select based on assignment.
GaussianMixture::shared_ptr gm = this->atMixture(idx);
conditional_error = gm->error(continuousValues);
// Iterate over each conditional.
for (auto &&conditional : *this) {
if (conditional->isHybrid()) {
// If conditional is hybrid, select based on assignment and compute error.
GaussianMixture::shared_ptr gm = conditional->asMixture();
AlgebraicDecisionTree<Key> conditional_error =
gm->error(continuousValues);
if (idx == 0) {
error_tree = conditional_error;
} else {
error_tree = error_tree + conditional_error;
}
error_tree = error_tree + conditional_error;
} else if (factors_.at(idx)->isContinuous()) {
} else if (conditional->isContinuous()) {
// If continuous only, get the (double) error
// and add it to the error_tree
double error = this->atGaussian(idx)->error(continuousValues);
double error = conditional->asGaussian()->error(continuousValues);
// Add the computed error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
} else if (factors_.at(idx)->isDiscrete()) {
// If factor at `idx` is discrete-only, we skip.
} else if (conditional->isDiscrete()) {
// Conditional is discrete-only, we skip.
continue;
}
}

52
gtsam/hybrid/HybridBayesNet.h
View File

@ -8,7 +8,7 @@
/**
* @file HybridBayesNet.h
* @brief A bayes net of Gaussian Conditionals indexed by discrete keys.
* @brief A Bayes net of Gaussian Conditionals indexed by discrete keys.
* @author Varun Agrawal
* @author Fan Jiang
* @author Frank Dellaert
@ -43,7 +43,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/// @name Standard Constructors
/// @{
/** Construct empty bayes net */
/** Construct empty Bayes net */
HybridBayesNet() = default;
/// @}
@ -120,7 +120,47 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
*/
DecisionTreeFactor::shared_ptr discreteConditionals() const;
public:
/**
* @brief Sample from an incomplete BayesNet, given missing variables.
*
* Example:
* std::mt19937_64 rng(42);
* VectorValues given = ...;
* auto sample = bn.sample(given, &rng);
*
* @param given Values of missing variables.
* @param rng The pseudo-random number generator.
* @return HybridValues
*/
HybridValues sample(const HybridValues &given, std::mt19937_64 *rng) const;
/**
* @brief Sample using ancestral sampling.
*
* Example:
* std::mt19937_64 rng(42);
* auto sample = bn.sample(&rng);
*
* @param rng The pseudo-random number generator.
* @return HybridValues
*/
HybridValues sample(std::mt19937_64 *rng) const;
/**
* @brief Sample from an incomplete BayesNet, use default rng.
*
* @param given Values of missing variables.
* @return HybridValues
*/
HybridValues sample(const HybridValues &given) const;
/**
* @brief Sample using ancestral sampling, use default rng.
*
* @return HybridValues
*/
HybridValues sample() const;
/// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves.
HybridBayesNet prune(size_t maxNrLeaves);
@ -145,8 +185,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
/**
* @brief Compute unnormalized probability for each discrete assignment,
* and return as a tree.
* @brief Compute unnormalized probability q(μ|M),
* for each discrete assignment, and return as a tree.
* q(μ|M) is the unnormalized probability at the MLE point μ,
* conditioned on the discrete variables.
*
* @param continuousValues Continuous values at which to compute the
* probability.

5
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -448,6 +448,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree(0.0);
// Iterate over each factor.
for (size_t idx = 0; idx < size(); idx++) {
AlgebraicDecisionTree<Key> factor_error;
@ -455,8 +456,10 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
// If factor is hybrid, select based on assignment.
GaussianMixtureFactor::shared_ptr gaussianMixture =
boost::static_pointer_cast<GaussianMixtureFactor>(factors_.at(idx));
// Compute factor error.
factor_error = gaussianMixture->error(continuousValues);
// If first factor, assign error, else add it.
if (idx == 0) {
error_tree = factor_error;
} else {
@ -470,7 +473,9 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
boost::static_pointer_cast<HybridGaussianFactor>(factors_.at(idx));
GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner();
// Compute the error of the gaussian factor.
double error = gaussian->error(continuousValues);
// Add the gaussian factor error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });

9
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -100,11 +100,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values
using Indices = KeyVector; ///< map from keys to values
/// @name Constructors
/// @{
/// @brief Default constructor.
HybridGaussianFactorGraph() = default;
/**
@ -175,6 +176,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
* @brief Compute error for each discrete assignment,
* and return as a tree.
*
* Error \f$ e = \Vert x - \mu \Vert_{\Sigma} \f$.
*
* @param continuousValues Continuous values at which to compute the error.
* @return AlgebraicDecisionTree<Key>
*/
@ -194,8 +197,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
const DiscreteValues& discreteValues) const;
/**
* @brief Compute unnormalized probability for each discrete assignment,
* and return as a tree.
* @brief Compute unnormalized probability \f$ P(X | M, Z) \f$
* for each discrete assignment, and return as a tree.
*
* @param continuousValues Continuous values at which to compute the
* probability.

51
gtsam/hybrid/MixtureFactor.h
View File

@ -23,6 +23,7 @@
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h>
#include <algorithm>
@ -86,11 +87,11 @@ class MixtureFactor : public HybridFactor {
* elements based on the number of discrete keys and the cardinality of the
* keys, so that the decision tree is constructed appropriately.
*
* @tparam FACTOR The type of the factor shared pointers being passed in. Will
* be typecast to NonlinearFactor shared pointers.
* @tparam FACTOR The type of the factor shared pointers being passed in.
* Will be typecast to NonlinearFactor shared pointers.
* @param keys Vector of keys for continuous factors.
* @param discreteKeys Vector of discrete keys.
* @param factors Vector of shared pointers to factors.
* @param factors Vector of nonlinear factors.
* @param normalized Flag indicating if the factor error is already
* normalized.
*/
@ -128,14 +129,15 @@ class MixtureFactor : public HybridFactor {
/**
* @brief Compute error of the MixtureFactor as a tree.
*
* @param continuousVals The continuous values for which to compute the error.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
* @param continuousValues The continuous values for which to compute the
* error.
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
* as the factor, and leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const Values& continuousVals) const {
AlgebraicDecisionTree<Key> error(const Values& continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousVals](const sharedFactor& factor) {
return factor->error(continuousVals);
auto errorFunc = [continuousValues](const sharedFactor& factor) {
return factor->error(continuousValues);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
@ -144,19 +146,19 @@ class MixtureFactor : public HybridFactor {
/**
* @brief Compute error of factor given both continuous and discrete values.
*
* @param continuousVals The continuous Values.
* @param discreteVals The discrete Values.
* @param continuousValues The continuous Values.
* @param discreteValues The discrete Values.
* @return double The error of this factor.
*/
double error(const Values& continuousVals,
const DiscreteValues& discreteVals) const {
// Retrieve the factor corresponding to the assignment in discreteVals.
auto factor = factors_(discreteVals);
double error(const Values& continuousValues,
const DiscreteValues& discreteValues) const {
// Retrieve the factor corresponding to the assignment in discreteValues.
auto factor = factors_(discreteValues);
// Compute the error for the selected factor
const double factorError = factor->error(continuousVals);
const double factorError = factor->error(continuousValues);
if (normalized_) return factorError;
return factorError +
this->nonlinearFactorLogNormalizingConstant(factor, continuousVals);
return factorError + this->nonlinearFactorLogNormalizingConstant(
factor, continuousValues);
}
size_t dim() const {
@ -212,17 +214,18 @@ class MixtureFactor : public HybridFactor {
/// Linearize specific nonlinear factors based on the assignment in
/// discreteValues.
GaussianFactor::shared_ptr linearize(
const Values& continuousVals, const DiscreteValues& discreteVals) const {
auto factor = factors_(discreteVals);
return factor->linearize(continuousVals);
const Values& continuousValues,
const DiscreteValues& discreteValues) const {
auto factor = factors_(discreteValues);
return factor->linearize(continuousValues);
}
/// Linearize all the continuous factors to get a GaussianMixtureFactor.
boost::shared_ptr<GaussianMixtureFactor> linearize(
const Values& continuousVals) const {
const Values& continuousValues) const {
// functional to linearize each factor in the decision tree
auto linearizeDT = [continuousVals](const sharedFactor& factor) {
return factor->linearize(continuousVals);
auto linearizeDT = [continuousValues](const sharedFactor& factor) {
return factor->linearize(continuousValues);
};
DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(

12
gtsam/hybrid/hybrid.i
View File

@ -196,22 +196,24 @@ class HybridNonlinearFactorGraph {
#include <gtsam/hybrid/MixtureFactor.h>
class MixtureFactor : gtsam::HybridFactor {
MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors, bool normalized = false);
MixtureFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors,
bool normalized = false);
template <FACTOR = {gtsam::NonlinearFactor}>
MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const std::vector<FACTOR*>& factors,
bool normalized = false);
double error(const gtsam::Values& continuousVals,
const gtsam::DiscreteValues& discreteVals) const;
double error(const gtsam::Values& continuousValues,
const gtsam::DiscreteValues& discreteValues) const;
double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor,
const gtsam::Values& values) const;
GaussianMixtureFactor* linearize(
const gtsam::Values& continuousVals) const;
const gtsam::Values& continuousValues) const;
void print(string s = "MixtureFactor\n",
const gtsam::KeyFormatter& keyFormatter =

11
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -104,7 +104,7 @@ TEST(GaussianMixture, Error) {
X(2), S2, model);
// Create decision tree
DiscreteKey m1(1, 2);
DiscreteKey m1(M(1), 2);
GaussianMixture::Conditionals conditionals(
{m1},
vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
@ -115,12 +115,19 @@ TEST(GaussianMixture, Error) {
values.insert(X(2), Vector2::Zero());
auto error_tree = mixture.error(values);
// regression
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> leaves = {0.5, 4.3252595};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
// regression
EXPECT(assert_equal(expected_error, error_tree, 1e-6));
// Regression for non-tree version.
DiscreteValues assignment;
assignment[M(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8);
assignment[M(1)] = 1;
EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8);
}
/* ************************************************************************* */

17
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -170,24 +170,25 @@ TEST(GaussianMixtureFactor, Error) {
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
VectorValues continuousVals;
continuousVals.insert(X(1), Vector2(0, 0));
continuousVals.insert(X(2), Vector2(1, 1));
VectorValues continuousValues;
continuousValues.insert(X(1), Vector2(0, 0));
continuousValues.insert(X(2), Vector2(1, 1));
// error should return a tree of errors, with nodes for each discrete value.
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousValues);
std::vector<DiscreteKey> discrete_keys = {m1};
// Error values for regression test
std::vector<double> errors = {1, 4};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
EXPECT(assert_equal(expected_error, error_tree));
// Test for single leaf given discrete assignment P(X|M,Z).
DiscreteValues discreteVals;
discreteVals[m1.first] = 1;
EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals),
1e-9);
DiscreteValues discreteValues;
discreteValues[m1.first] = 1;
EXPECT_DOUBLES_EQUAL(
4.0, mixtureFactor.error(continuousValues, discreteValues), 1e-9);
}
/* ************************************************************************* */

72
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -197,8 +197,7 @@ TEST(HybridBayesNet, Error) {
// Verify error computation and check for specific error value
DiscreteValues discrete_values;
discrete_values[M(0)] = 1;
discrete_values[M(1)] = 1;
boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1);
double total_error = 0;
for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {
@ -298,6 +297,75 @@ TEST(HybridBayesNet, Serialization) {
EXPECT(equalsBinary<HybridBayesNet>(hbn));
}
/* ****************************************************************************/
// Test HybridBayesNet sampling.
TEST(HybridBayesNet, Sampling) {
HybridNonlinearFactorGraph nfg;
auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
auto zero_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_hybrid<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKey mode(M(0), 2);
auto discrete_prior = boost::make_shared<DiscreteDistribution>(mode, "1/1");
nfg.push_discrete(discrete_prior);
Values initial;
double z0 = 0.0, z1 = 1.0;
initial.insert<double>(X(0), z0);
initial.insert<double>(X(1), z1);
// Create the factor graph from the nonlinear factor graph.
HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial);
// Eliminate into BN
Ordering ordering = fg->getHybridOrdering();
HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering);
// Set up sampling
std::mt19937_64 gen(11);
// Initialize containers for computing the mean values.
vector<double> discrete_samples;
VectorValues average_continuous;
size_t num_samples = 1000;
for (size_t i = 0; i < num_samples; i++) {
// Sample
HybridValues sample = bn->sample(&gen);
discrete_samples.push_back(sample.discrete()[M(0)]);
if (i == 0) {
average_continuous.insert(sample.continuous());
} else {
average_continuous += sample.continuous();
}
}
EXPECT_LONGS_EQUAL(2, average_continuous.size());
EXPECT_LONGS_EQUAL(num_samples, discrete_samples.size());
// Regressions don't work across platforms :-(
// // regression for specific RNG seed
// double discrete_sum =
// std::accumulate(discrete_samples.begin(), discrete_samples.end(),
// decltype(discrete_samples)::value_type(0));
// EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9);
// VectorValues expected;
// expected.insert({X(0), Vector1(-0.0131207162712)});
// expected.insert({X(1), Vector1(-0.499026377568)});
// // regression for specific RNG seed
// EXPECT(assert_equal(expected, average_continuous.scale(1.0 /
// num_samples)));
}
/* ************************************************************************* */
int main() {
TestResult tr;

11
gtsam/hybrid/tests/testMixtureFactor.cpp
View File

@ -41,7 +41,8 @@ TEST(MixtureFactor, Constructor) {
CHECK(it == factor.end());
}
/* ************************************************************************* */
// Test .print() output.
TEST(MixtureFactor, Printing) {
DiscreteKey m1(1, 2);
double between0 = 0.0;
@ -87,11 +88,11 @@ TEST(MixtureFactor, Error) {
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
Values continuousVals;
continuousVals.insert<double>(X(1), 0);
continuousVals.insert<double>(X(2), 1);
Values continuousValues;
continuousValues.insert<double>(X(1), 0);
continuousValues.insert<double>(X(2), 1);
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousValues);
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> errors = {0.5, 0};

8
gtsam/inference/graph-inl.h
View File

@ -200,8 +200,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
KEY key1 = factor->key1();
KEY key2 = factor->key2();
KEY key1 = factor->template key<1>();
KEY key2 = factor->template key<2>();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
FACTOR2>(factor);
if (!factor2) continue;
KEY key1 = factor2->key1();
KEY key2 = factor2->key2();
KEY key1 = factor2->template key<1>();
KEY key2 = factor2->template key<2>();
// if the tree contains the key
if ((tree.find(key1) != tree.end() &&
tree.find(key1)->second.compare(key2) == 0) ||

14
gtsam/linear/GaussianBayesNet.cpp
View File

@ -64,8 +64,9 @@ namespace gtsam {
return sample(result, rng);
}
VectorValues GaussianBayesNet::sample(VectorValues result,
VectorValues GaussianBayesNet::sample(const VectorValues& given,
std::mt19937_64* rng) const {
VectorValues result(given);
// sample each node in reverse topological sort order (parents first)
for (auto cg : boost::adaptors::reverse(*this)) {
const VectorValues sampled = cg->sample(result, rng);
@ -79,7 +80,7 @@ namespace gtsam {
return sample(&kRandomNumberGenerator);
}
VectorValues GaussianBayesNet::sample(VectorValues given) const {
VectorValues GaussianBayesNet::sample(const VectorValues& given) const {
return sample(given, &kRandomNumberGenerator);
}
@ -217,14 +218,7 @@ namespace gtsam {
double GaussianBayesNet::logDeterminant() const {
double logDet = 0.0;
for (const sharedConditional& cg : *this) {
if (cg->get_model()) {
Vector diag = cg->R().diagonal();
cg->get_model()->whitenInPlace(diag);
logDet += diag.unaryExpr([](double x) { return log(x); }).sum();
} else {
logDet +=
cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
}
logDet += cg->logDeterminant();
}
return logDet;
}

4
gtsam/linear/GaussianBayesNet.h
View File

@ -110,13 +110,13 @@ namespace gtsam {
* VectorValues given = ...;
* auto sample = gbn.sample(given, &rng);
*/
VectorValues sample(VectorValues given, std::mt19937_64* rng) const;
VectorValues sample(const 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;
VectorValues sample(const VectorValues& given) const;
/**
* Return ordering corresponding to a topological sort.

2
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
double result = 0.0;
// this clique
result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
result += clique->conditional()->logDeterminant();
// sum of children
for(const typename BAYESTREE::sharedClique& child: clique->children_)

10
gtsam/linear/GaussianBayesTree.cpp
View File

@ -50,15 +50,7 @@ namespace gtsam {
const GaussianBayesTreeClique::shared_ptr& clique,
LogDeterminantData& parentSum) {
auto cg = clique->conditional();
double logDet;
if (cg->get_model()) {
Vector diag = cg->R().diagonal();
cg->get_model()->whitenInPlace(diag);
logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
} else {
logDet =
cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
}
double logDet = cg->logDeterminant();
// Add the current clique's log-determinant to the overall sum
(*parentSum.logDet) += logDet;
return parentSum;

24
gtsam/linear/GaussianConditional.cpp
View File

@ -155,6 +155,20 @@ namespace gtsam {
}
}
/* ************************************************************************* */
double GaussianConditional::logDeterminant() const {
double logDet;
if (this->get_model()) {
Vector diag = this->R().diagonal();
this->get_model()->whitenInPlace(diag);
logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
} else {
logDet =
this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
}
return logDet;
}
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const {
// Concatenate all vector values that correspond to parent variables
@ -285,14 +299,12 @@ namespace gtsam {
"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();
// The vector of sigma values for sampling.
// If no model, initialize sigmas to 1, else to model sigmas
const Vector& sigmas = (!model_) ? Vector::Ones(rows()) : model_->sigmas();
solution[key] += Sampler::sampleDiagonal(sigmas, rng);
return solution;
}

22
gtsam/linear/GaussianConditional.h
View File

@ -133,6 +133,28 @@ namespace gtsam {
/** Get a view of the r.h.s. vector d */
const constBVector d() const { return BaseFactor::getb(); }
/**
* @brief Compute the log determinant of the Gaussian conditional.
* The determinant is computed using the R matrix, which is upper
* triangular.
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
* @return double
*/
double logDeterminant() const;
/**
* @brief Compute the determinant of the conditional from the
* upper-triangular R matrix.
*
* The determinant is computed in log form (hence summation) for numerical
* stability and then exponentiated.
*
* @return double
*/
double determinant() const { return exp(this->logDeterminant()); }
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are

2
gtsam/mainpage.dox
View File

@ -22,7 +22,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto
-# The number of variables your factor involves is <b>unknown</b> at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError()
- This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel).
-# The number of variables your factor involves is <b>known</b> at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement <b>\c evaluateError()</b>
-# The number of variables your factor involves is <b>known</b> at compile time, derive from NoiseModelFactorN<T1, T2, ...> (where T1, T2, ... are the types of the variables, e.g. double, Vector, Pose3) and implement <b>\c evaluateError()</b>.
- This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor.
-# Derive from NonlinearFactor
- This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor.

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -105,8 +105,8 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
//------------------------------------------------------------------------------
void AHRSFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ",";
_PIM_.print(" preintegrated measurements:");
noiseModel_->print(" noise model: ");
}

5
gtsam/navigation/AHRSFactor.h
View File

@ -128,10 +128,10 @@ private:
}
};
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
PreintegratedAhrsMeasurements _PIM_;
@ -212,6 +212,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));

10
gtsam/navigation/AttitudeFactor.h
View File

@ -76,9 +76,9 @@ public:
* Version of AttitudeFactor for Rot3
* @ingroup navigation
*/
class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor {
class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
typedef NoiseModelFactor1<Rot3> Base;
typedef NoiseModelFactorN<Rot3> Base;
public:
@ -132,6 +132,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor",
@ -149,10 +150,10 @@ template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFacto
* Version of AttitudeFactor for Pose3
* @ingroup navigation
*/
class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
public AttitudeFactor {
typedef NoiseModelFactor1<Pose3> Base;
typedef NoiseModelFactorN<Pose3> Base;
public:
@ -212,6 +213,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor",

4
gtsam/navigation/BarometricFactor.cpp
View File

@ -26,8 +26,8 @@ namespace gtsam {
void BarometricFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? "" : s + " ") << "Barometric Factor on "
<< keyFormatter(key1()) << "Barometric Bias on "
<< keyFormatter(key2()) << "\n";
<< keyFormatter(key<1>()) << "Barometric Bias on "
<< keyFormatter(key<2>()) << "\n";
cout << " Baro measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: ");

5
gtsam/navigation/BarometricFactor.h
View File

@ -31,9 +31,9 @@ namespace gtsam {
* https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
* @ingroup navigation
*/
class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
private:
typedef NoiseModelFactor2<Pose3, double> Base;
typedef NoiseModelFactorN<Pose3, double> Base;
double nT_; ///< Height Measurement based on a standard atmosphere
@ -99,6 +99,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -204,9 +204,9 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
void CombinedImuFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
<< keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << ","
<< keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>())
<< ")\n";
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");

8
gtsam/navigation/CombinedImuFactor.h
View File

@ -255,14 +255,14 @@ public:
*
* @ingroup navigation
*/
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public:
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base;
PreintegratedCombinedMeasurements _PIM_;
@ -334,7 +334,9 @@ public:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
// NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(_PIM_);
}

6
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -26,15 +26,15 @@ namespace gtsam {
* Binary factor for applying a constant velocity model to a moving body represented as a NavState.
* The only measurement is dt, the time delta between the states.
*/
class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
double dt_;
public:
using Base = NoiseModelFactor2<NavState, NavState>;
using Base = NoiseModelFactorN<NavState, NavState>;
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
: NoiseModelFactor2<NavState, NavState>(model, i, j), dt_(dt) {}
: NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
~ConstantVelocityFactor() override{};
/**

10
gtsam/navigation/GPSFactor.h
View File

@ -32,11 +32,11 @@ namespace gtsam {
* See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1<Pose3> {
class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
private:
typedef NoiseModelFactor1<Pose3> Base;
typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates
@ -99,6 +99,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
@ -110,11 +111,11 @@ private:
* Version of GPSFactor for NavState
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1<NavState> {
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
private:
typedef NoiseModelFactor1<NavState> Base;
typedef NoiseModelFactorN<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates
@ -163,6 +164,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));

24
gtsam/navigation/ImuFactor.cpp
View File

@ -133,9 +133,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
//------------------------------------------------------------------------------
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
<< "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>())
<< "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
<< "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>())
<< ")\n";
cout << *this << endl;
}
@ -184,22 +184,22 @@ PreintegratedImuMeasurements ImuFactor::Merge(
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) {
// IMU bias keys must be the same.
if (f01->key5() != f12->key5())
if (f01->key<5>() != f12->key<5>())
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
// expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>())
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor
auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0
f01->key2(), // V0
f12->key3(), // P2
f12->key4(), // V2
f01->key5(), // B
return boost::make_shared<ImuFactor>(f01->key<1>(), // P0
f01->key<2>(), // V0
f12->key<3>(), // P2
f12->key<4>(), // V2
f01->key<5>(), // B
pim02);
}
#endif
@ -230,8 +230,8 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "ImuFactor2("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ")\n";
<< keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key<3>()) << ")\n";
cout << *this << endl;
}

10
gtsam/navigation/ImuFactor.h
View File

@ -168,12 +168,12 @@ public:
*
* @ingroup navigation
*/
class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> {
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_;
@ -248,6 +248,7 @@ public:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
@ -259,11 +260,11 @@ public:
* ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
* @ingroup navigation
*/
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
private:
typedef ImuFactor2 This;
typedef NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> Base;
typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_;
@ -316,6 +317,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);

16
gtsam/navigation/MagFactor.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* and assumes scale, direction, and the bias are given.
* Rotation is around negative Z axis, i.e. positive is yaw to right!
*/
class MagFactor: public NoiseModelFactor1<Rot2> {
class MagFactor: public NoiseModelFactorN<Rot2> {
const Point3 measured_; ///< The measured magnetometer values
const Point3 nM_; ///< Local magnetic field (mag output units)
@ -50,7 +50,7 @@ public:
MagFactor(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) :
NoiseModelFactor1<Rot2>(model, key), //
NoiseModelFactorN<Rot2>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) {
}
@ -87,7 +87,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias
* and assumes scale, direction, and the bias are given
*/
class MagFactor1: public NoiseModelFactor1<Rot3> {
class MagFactor1: public NoiseModelFactorN<Rot3> {
const Point3 measured_; ///< The measured magnetometer values
const Point3 nM_; ///< Local magnetic field (mag output units)
@ -99,7 +99,7 @@ public:
MagFactor1(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) :
NoiseModelFactor1<Rot3>(model, key), //
NoiseModelFactorN<Rot3>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) {
}
@ -125,7 +125,7 @@ public:
* This version uses model measured bM = bRn * nM + bias
* and optimizes for both nM and the bias, where nM is in units defined by magnetometer
*/
class MagFactor2: public NoiseModelFactor2<Point3, Point3> {
class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -135,7 +135,7 @@ public:
/** Constructor */
MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
const SharedNoiseModel& model) :
NoiseModelFactor2<Point3, Point3>(model, key1, key2), //
NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
measured_(measured), bRn_(nRb.inverse()) {
}
@ -166,7 +166,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias.
*/
class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -176,7 +176,7 @@ public:
/** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) {
}

5
gtsam/navigation/MagPoseFactor.h
View File

@ -25,10 +25,10 @@ namespace gtsam {
* expressed in the sensor frame.
*/
template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> {
class MagPoseFactor: public NoiseModelFactorN<POSE> {
private:
using This = MagPoseFactor<POSE>;
using Base = NoiseModelFactor1<POSE>;
using Base = NoiseModelFactorN<POSE>;
using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
using Rot = typename POSE::Rotation;
@ -129,6 +129,7 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -53,8 +53,8 @@ class ExtendedKalmanFilter {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor;
typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactorN<VALUE> MeasurementFactor;
#endif
protected:

16
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -56,9 +56,9 @@ namespace gtsam {
* MultiplyFunctor(multiplier));
*/
template <typename R, typename T>
class FunctorizedFactor : public NoiseModelFactor1<T> {
class FunctorizedFactor : public NoiseModelFactorN<T> {
private:
using Base = NoiseModelFactor1<T>;
using Base = NoiseModelFactorN<T>;
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl;
<< keyFormatter(this->template key<1>()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
@ -120,6 +120,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
@ -155,9 +156,9 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
* @param T2: The second argument type for the functor.
*/
template <typename R, typename T1, typename T2>
class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
private:
using Base = NoiseModelFactor2<T1, T2>;
using Base = NoiseModelFactorN<T1, T2>;
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
@ -207,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
<< keyFormatter(this->key1()) << ", "
<< keyFormatter(this->key2()) << ")" << std::endl;
<< keyFormatter(this->template key<1>()) << ", "
<< keyFormatter(this->template key<2>()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
@ -227,6 +228,7 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);

15
gtsam/nonlinear/NonlinearEquality.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<class VALUE>
class NonlinearEquality: public NoiseModelFactor1<VALUE> {
class NonlinearEquality: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
@ -62,7 +62,7 @@ private:
using This = NonlinearEquality<VALUE>;
// typedef to base class
using Base = NoiseModelFactor1<VALUE>;
using Base = NoiseModelFactorN<VALUE>;
public:
@ -184,6 +184,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
@ -203,13 +204,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUE>
class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE X;
protected:
typedef NoiseModelFactor1<VALUE> Base;
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEquality1<VALUE> This;
/// Default constructor to allow for serialization
@ -272,6 +273,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
@ -290,9 +292,9 @@ struct traits<NonlinearEquality1<VALUE> >
* be the same.
*/
template <class T>
class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
protected:
using Base = NoiseModelFactor2<T, T>;
using Base = NoiseModelFactorN<T, T>;
using This = NonlinearEquality2<T>;
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
@ -337,6 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
}

907
gtsam/nonlinear/NonlinearFactor.h
View File

File diff suppressed because it is too large Load Diff

5
gtsam/nonlinear/PriorFactor.h
View File

@ -27,14 +27,14 @@ namespace gtsam {
* @ingroup nonlinear
*/
template<class VALUE>
class PriorFactor: public NoiseModelFactor1<VALUE> {
class PriorFactor: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
private:
typedef NoiseModelFactor1<VALUE> Base;
typedef NoiseModelFactorN<VALUE> Base;
VALUE prior_; /** The measurement */
@ -105,6 +105,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);

77
gtsam/nonlinear/tests/priorFactor.xml Normal file
View File

@ -0,0 +1,77 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="15">
<data class_id="0" tracking_level="1" version="0" object_id="_0">
<NoiseModelFactor1 class_id="1" tracking_level="0" version="0">
<NoiseModelFactor class_id="2" tracking_level="0" version="0">
<NonlinearFactor class_id="3" tracking_level="0" version="0">
<keys_>
<count>1</count>
<item_version>0</item_version>
<item>12345</item>
</keys_>
</NonlinearFactor>
<noiseModel_ class_id="5" tracking_level="0" version="1">
<px class_id="6" class_name="gtsam_noiseModel_Unit" tracking_level="1" version="0" object_id="_1">
<Isotropic class_id="7" tracking_level="1" version="0" object_id="_2">
<Diagonal class_id="8" tracking_level="1" version="0" object_id="_3">
<Gaussian class_id="9" tracking_level="0" version="0">
<Base class_id="10" tracking_level="0" version="0">
<dim_>6</dim_>
</Base>
<sqrt_information_ class_id="11" tracking_level="0" version="1">
<initialized>0</initialized>
</sqrt_information_>
</Gaussian>
<sigmas_ class_id="12" tracking_level="0" version="0">
<size>6</size>
<data>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
</data>
</sigmas_>
<invsigmas_>
<size>6</size>
<data>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
</data>
</invsigmas_>
</Diagonal>
<sigma_>1.00000000000000000e+00</sigma_>
<invsigma_>1.00000000000000000e+00</invsigma_>
</Isotropic>
</px>
</noiseModel_>
</NoiseModelFactor>
</NoiseModelFactor1>
<prior_ class_id="13" tracking_level="0" version="0">
<R_ class_id="14" tracking_level="0" version="0">
<rot11>4.11982245665682978e-01</rot11>
<rot12>-8.33737651774156818e-01</rot12>
<rot13>-3.67630462924899259e-01</rot13>
<rot21>-5.87266449276209815e-02</rot21>
<rot22>-4.26917621276207360e-01</rot22>
<rot23>9.02381585483330806e-01</rot23>
<rot31>-9.09297426825681709e-01</rot31>
<rot32>-3.50175488374014632e-01</rot32>
<rot33>-2.24845095366152908e-01</rot33>
</R_>
<t_ class_id="15" tracking_level="0" version="0">
<data>
<item>4.00000000000000000e+00</item>
<item>5.00000000000000000e+00</item>
<item>6.00000000000000000e+00</item>
</data>
</t_>
</prior_>
</data>
</boost_serialization>

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

@ -107,8 +107,60 @@ TEST (Serialization, TemplatedValues) {
EXPECT(equalsBinary(values));
}
TEST(Serialization, ISAM2) {
/**
* Test deserializing from a known serialization generated by code from commit
* 0af17f438f62f4788f3a572ecd36d06d224fd1e1 (>4.2a7)
* We only test that deserialization matches since
* (1) that's the main backward compatibility requirement and
* (2) serialized string depends on boost version
* Also note: we don't run this test when quaternions or TBB are enabled since
* serialization structures are different and the serialized strings/xml are
* hard-coded in this test.
*/
TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
PriorFactor<Pose3> factor(
12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)),
noiseModel::Unit::Create(6));
// roundtrip (sanity check)
PriorFactor<Pose3> factor_deserialized_str_2 = PriorFactor<Pose3>();
roundtrip(factor, factor_deserialized_str_2);
EXPECT(assert_equal(factor, factor_deserialized_str_2));
#if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB)
// Deserialize string
std::string serialized_str =
"22 serialization::archive 15 1 0\n"
"0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n"
"1 1 0\n"
"2 1 0\n"
"3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 6 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 "
"-8.33737651774156818e-01 -3.67630462924899259e-01 "
"-5.87266449276209815e-02 -4.26917621276207360e-01 "
"9.02381585483330806e-01 -9.09297426825681709e-01 "
"-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 "
"4.00000000000000000e+00 5.00000000000000000e+00 "
"6.00000000000000000e+00\n";
PriorFactor<Pose3> factor_deserialized_str = PriorFactor<Pose3>();
deserializeFromString(serialized_str, factor_deserialized_str);
EXPECT(assert_equal(factor, factor_deserialized_str));
// Deserialize XML
PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR
"/../../gtsam/nonlinear/tests/priorFactor.xml",
factor_deserialized_xml);
EXPECT(assert_equal(factor, factor_deserialized_xml));
#endif
}
TEST(Serialization, ISAM2) {
gtsam::ISAM2Params parameters;
gtsam::ISAM2 solver(parameters);
gtsam::NonlinearFactorGraph graph;

4
gtsam/sfm/ShonanAveraging.cpp
View File

@ -958,7 +958,7 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
// the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
// because the tangent space of Pose2 is ordered as (vx, vy, w)
auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
return BinaryMeasurement<Rot2>(f->key<1>(), f->key<2>(), f->measured().rotation(),
model);
}
@ -1006,7 +1006,7 @@ static BinaryMeasurement<Rot3> convert(
// the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
// because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
return BinaryMeasurement<Rot3>(f->key<1>(), f->key<2>(), f->measured().rotation(),
model);
}

8
gtsam/sfm/ShonanFactor.cpp
View File

@ -35,7 +35,7 @@ template <size_t d>
ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
: NoiseModelFactorN<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
M_(R12.matrix()), // d*d in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
@ -57,8 +57,8 @@ ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
template <size_t d>
void ShonanFactor<d>::print(const std::string &s,
const KeyFormatter &keyFormatter) const {
std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << ","
<< keyFormatter(key<2>()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
@ -68,7 +68,7 @@ template <size_t d>
bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const ShonanFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
return e != nullptr && NoiseModelFactorN<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}

4
gtsam/sfm/ShonanFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
*/
template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> {
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
@ -66,7 +66,7 @@ public:
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @name NoiseModelFactorN methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]

4
gtsam/sfm/TranslationFactor.h
View File

@ -39,9 +39,9 @@ namespace gtsam {
*
*
*/
class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
private:
typedef NoiseModelFactor2<Point3, Point3> Base;
typedef NoiseModelFactorN<Point3, Point3> Base;
Point3 measured_w_aZb_;
public:

11
gtsam/slam/BetweenFactor.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class VALUE>
class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {
class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
// Check that VALUE type is a testable Lie group
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
@ -50,7 +50,7 @@ namespace gtsam {
private:
typedef BetweenFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base;
typedef NoiseModelFactorN<VALUE, VALUE> Base;
VALUE measured_; /** The measurement */
@ -88,8 +88,8 @@ namespace gtsam {
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n";
traits<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: ");
}
@ -101,7 +101,7 @@ namespace gtsam {
}
/// @}
/// @name NoiseModelFactor2 methods
/// @name NoiseModelFactorN methods
/// @{
/// evaluate error, returns vector of errors size of tangent space
@ -136,6 +136,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

12
gtsam/slam/BoundingConstraint.h
View File

@ -30,9 +30,9 @@ namespace gtsam {
* @ingroup slam
*/
template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
typedef VALUE X;
typedef NoiseModelFactor1<VALUE> Base;
typedef NoiseModelFactorN<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
double threshold_;
@ -85,6 +85,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
@ -97,11 +98,11 @@ private:
* to implement for specific systems
*/
template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
double threshold_;
@ -128,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
double x = value(c.at<X1>(this->template key<1>()), c.at<X2>(this->template key<2>()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
@ -158,6 +159,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);

4
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -27,8 +27,8 @@ namespace gtsam {
/* ************************************************************************* */
void EssentialMatrixConstraint::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
<< "," << keyFormatter(this->key2()) << ")\n";
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>())
<< "," << keyFormatter(this->key<2>()) << ")\n";
measuredE_.print(" measured: ");
this->noiseModel_->print(" noise model: ");
}

5
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
* Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
* @ingroup slam
*/
class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
private:
typedef EssentialMatrixConstraint This;
typedef NoiseModelFactor2<Pose3, Pose3> Base;
typedef NoiseModelFactorN<Pose3, Pose3> Base;
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
@ -93,6 +93,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));

12
gtsam/slam/EssentialMatrixFactor.h
View File

@ -31,10 +31,10 @@ namespace gtsam {
/**
* Factor that evaluates epipolar error p'Ep for given essential matrix
*/
class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
typedef NoiseModelFactor1<EssentialMatrix> Base;
typedef NoiseModelFactorN<EssentialMatrix> Base;
typedef EssentialMatrixFactor This;
public:
@ -106,12 +106,12 @@ class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
* in image 2 is perfect, and returns re-projection error in image 1
*/
class EssentialMatrixFactor2
: public NoiseModelFactor2<EssentialMatrix, double> {
: public NoiseModelFactorN<EssentialMatrix, double> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, double> Base;
typedef NoiseModelFactorN<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This;
public:
@ -321,11 +321,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
*/
template <class CALIBRATION>
class EssentialMatrixFactor4
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
: public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
private:
Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixFactor4 This;
static constexpr int DimK = FixedDimension<CALIBRATION>::value;

20
gtsam/slam/FrobeniusFactor.h
View File

@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
class FrobeniusPrior : public NoiseModelFactorN<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
@ -59,7 +59,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
: NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
@ -75,13 +75,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
: NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
j2) {}
/// Error is just Frobenius norm between rotation matrices.
@ -101,7 +101,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/
template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
@ -116,7 +116,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// Construct from two keys and measured rotation
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
: NoiseModelFactorN<Rot, Rot>(
ConvertNoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
<< ">(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
<< ">(" << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n";
traits<Rot>::Print(R12_, " R12: ");
this->noiseModel_->print(" noise model: ");
}
@ -140,12 +140,12 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override {
auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
traits<Rot>::Equals(this->R12_, e->R12_, tol);
}
/// @}
/// @name NoiseModelFactor2 methods
/// @name NoiseModelFactorN methods
/// @{
/// Error is Frobenius norm between R1*R12 and R2.

16
gtsam/slam/GeneralSFMFactor.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
@ -74,7 +74,7 @@ protected:
public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class
typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -140,7 +140,7 @@ public:
// Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2();
const Key key1 = this->template key<1>(), key2 = this->template key<2>();
JacobianC H1;
JacobianL H2;
Vector2 b;
@ -184,6 +184,7 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
@ -200,7 +201,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
*/
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value;
@ -213,7 +214,7 @@ public:
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -269,8 +270,8 @@ public:
if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>())
<< " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
}
return Z_2x1;
}
@ -285,6 +286,7 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/slam/InitializePose3.cpp
View File

@ -228,7 +228,7 @@ void InitializePose3::createSymbolicGraph(
Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap->emplace(factorId, Rij);
Key key1 = pose3Between->key1();
Key key1 = pose3Between->key<1>();
if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key1).push_back(factorId);
} else {
@ -236,7 +236,7 @@ void InitializePose3::createSymbolicGraph(
edge_id.push_back(factorId);
adjEdgesMap->emplace(key1, edge_id);
}
Key key2 = pose3Between->key2();
Key key2 = pose3Between->key<2>();
if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key2).push_back(factorId);
} else {

4
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -15,8 +15,8 @@ namespace gtsam {
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << (s == "" ? "" : "\n");
cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
<< keyFormatter(key2()) << ")\n";
cout << "OrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", "
<< keyFormatter(key<2>()) << ")\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}

8
gtsam/slam/OrientedPlane3Factor.h
View File

@ -15,10 +15,10 @@ namespace gtsam {
/**
* Factor to measure a planar landmark from a given pose
*/
class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
public:
/// Constructor
@ -49,10 +49,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, Oriente
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> {
class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
typedef NoiseModelFactor1<OrientedPlane3> Base;
typedef NoiseModelFactorN<OrientedPlane3> Base;
public:
typedef OrientedPlane3DirectionPrior This;

5
gtsam/slam/PoseRotationPrior.h
View File

@ -16,11 +16,11 @@
namespace gtsam {
template<class POSE>
class PoseRotationPrior : public NoiseModelFactor1<POSE> {
class PoseRotationPrior : public NoiseModelFactorN<POSE> {
public:
typedef PoseRotationPrior<POSE> This;
typedef NoiseModelFactor1<POSE> Base;
typedef NoiseModelFactorN<POSE> Base;
typedef POSE Pose;
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
@ -92,6 +92,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

5
gtsam/slam/PoseTranslationPrior.h
View File

@ -18,10 +18,10 @@ namespace gtsam {
* A prior on the translation part of a pose
*/
template<class POSE>
class PoseTranslationPrior : public NoiseModelFactor1<POSE> {
class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
public:
typedef PoseTranslationPrior<POSE> This;
typedef NoiseModelFactor1<POSE> Base;
typedef NoiseModelFactorN<POSE> Base;
typedef POSE Pose;
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
@ -91,6 +91,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

10
gtsam/slam/ProjectionFactor.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
*/
template <class POSE = Pose3, class LANDMARK = Point3,
class CALIBRATION = Cal3_S2>
class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -52,7 +52,7 @@ namespace gtsam {
public:
/// shorthand for base class type
typedef NoiseModelFactor2<POSE, LANDMARK> Base;
typedef NoiseModelFactorN<POSE, LANDMARK> Base;
/// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
@ -154,10 +154,10 @@ namespace gtsam {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_)
throw CheiralityException(this->key2());
throw CheiralityException(this->template key<2>());
}
return Vector2::Constant(2.0 * K_->fx());
}

16
gtsam/slam/ReferenceFrameFactor.h
View File

@ -54,13 +54,13 @@ P transform_point(
* specific classes of landmarks
*/
template<class POINT, class TRANSFORM>
class ReferenceFrameFactor : public NoiseModelFactor3<POINT, TRANSFORM, POINT> {
class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
protected:
/** default constructor for serialization only */
ReferenceFrameFactor() {}
public:
typedef NoiseModelFactor3<POINT, TRANSFORM, POINT> Base;
typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
typedef POINT Point;
@ -107,16 +107,16 @@ public:
void print(const std::string& s="",
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": ReferenceFrameFactor("
<< "Global: " << keyFormatter(this->key1()) << ","
<< " Transform: " << keyFormatter(this->key2()) << ","
<< " Local: " << keyFormatter(this->key3()) << ")\n";
<< "Global: " << keyFormatter(this->template key<1>()) << ","
<< " Transform: " << keyFormatter(this->template key<2>()) << ","
<< " Local: " << keyFormatter(this->template key<3>()) << ")\n";
this->noiseModel_->print(" noise model");
}
// access - convenience functions
Key global_key() const { return this->key1(); }
Key transform_key() const { return this->key2(); }
Key local_key() const { return this->key3(); }
Key global_key() const { return this->template key<1>(); }
Key transform_key() const { return this->template key<2>(); }
Key local_key() const { return this->template key<3>(); }
private:
/** Serialization function */

8
gtsam/slam/RotateFactor.h
View File

@ -20,11 +20,11 @@ namespace gtsam {
* with z and p measured and predicted angular velocities, and hence
* p = iRc * z
*/
class RotateFactor: public NoiseModelFactor1<Rot3> {
class RotateFactor: public NoiseModelFactorN<Rot3> {
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base;
typedef NoiseModelFactorN<Rot3> Base;
typedef RotateFactor This;
public:
@ -64,11 +64,11 @@ public:
* Factor on unknown rotation iRc that relates two directions c
* Directions provide less constraints than a full rotation
*/
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z
typedef NoiseModelFactor1<Rot3> Base;
typedef NoiseModelFactorN<Rot3> Base;
typedef RotateDirectionsFactor This;
public:

9
gtsam/slam/StereoFactor.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class POSE, class LANDMARK>
class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
private:
// Keep a copy of measurement and calibration for I/O
@ -43,7 +43,7 @@ private:
public:
// shorthand for base class type
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< typedef for base class
typedef NoiseModelFactorN<POSE, LANDMARK> Base; ///< typedef for base class
typedef GenericStereoFactor<POSE, LANDMARK> This; ///< typedef for this class (with templates)
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef POSE CamPose; ///< typedef for Pose Lie Value type
@ -141,8 +141,8 @@ public:
if (H1) *H1 = Matrix::Zero(3,6);
if (H2) *H2 = Z_3x3;
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_)
throw StereoCheiralityException(this->key2());
}
@ -170,6 +170,7 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/slam/TriangulationFactor.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class CAMERA>
class TriangulationFactor: public NoiseModelFactor1<Point3> {
class TriangulationFactor: public NoiseModelFactorN<Point3> {
public:
@ -40,7 +40,7 @@ public:
protected:
/// shorthand for base class type
using Base = NoiseModelFactor1<Point3>;
using Base = NoiseModelFactorN<Point3>;
/// shorthand for this class
using This = TriangulationFactor<CAMERA>;

12
gtsam/slam/dataset.cpp
View File

@ -535,7 +535,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
graph->push_back(*f);
// Insert vertices if pure odometry file
Key key1 = (*f)->key1(), key2 = (*f)->key2();
Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>();
if (!initial->exists(key1))
initial->insert(key1, Pose2());
if (!initial->exists(key2))
@ -603,7 +603,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
continue;
const Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
<< " " << RR(0, 2) << " " << RR(1, 2) << endl;
@ -691,8 +691,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
}
Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << index(factor->key1()) << " "
<< index(factor->key2()) << " " << pose.x() << " " << pose.y()
stream << "EDGE_SE2 " << index(factor->key<1>()) << " "
<< index(factor->key<2>()) << " " << pose.x() << " " << pose.y()
<< " " << pose.theta();
for (size_t i = 0; i < 3; i++) {
for (size_t j = i; j < 3; j++) {
@ -717,8 +717,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
const Pose3 pose3D = factor3D->measured();
const Point3 p = pose3D.translation();
const auto q = pose3D.rotation().toQuaternion();
stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " "
<< index(factor3D->key2()) << " " << p.x() << " " << p.y() << " "
stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " "
<< index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " "
<< p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
<< q.w();

4
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -22,9 +22,9 @@ namespace gtsam {
* assumed to be PoseRTV
*/
template<class POSE>
class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
class FullIMUFactor : public NoiseModelFactorN<POSE, POSE> {
public:
typedef NoiseModelFactor2<POSE, POSE> Base;
typedef NoiseModelFactorN<POSE, POSE> Base;
typedef FullIMUFactor<POSE> This;
protected:

4
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -20,9 +20,9 @@ namespace gtsam {
* assumed to be PoseRTV
*/
template<class POSE>
class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
class IMUFactor : public NoiseModelFactorN<POSE, POSE> {
public:
typedef NoiseModelFactor2<POSE, POSE> Base;
typedef NoiseModelFactorN<POSE, POSE> Base;
typedef IMUFactor<POSE> This;
protected:

16
gtsam_unstable/dynamics/Pendulum.h
View File

@ -20,11 +20,11 @@ namespace gtsam {
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
*/
class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
class PendulumFactor1: public NoiseModelFactorN<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<double, double, double> Base;
typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactor1() {}
@ -66,11 +66,11 @@ public:
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
*/
class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
class PendulumFactor2: public NoiseModelFactorN<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<double, double, double> Base;
typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactor2() {}
@ -113,11 +113,11 @@ public:
* \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
* \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
*/
class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
class PendulumFactorPk: public NoiseModelFactorN<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<double, double, double> Base;
typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactorPk() {}
@ -169,11 +169,11 @@ public:
* \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
* \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
*/
class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
class PendulumFactorPk1: public NoiseModelFactorN<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<double, double, double> Base;
typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactorPk1() {}

8
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -24,10 +24,10 @@ namespace gtsam {
* Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
* in sequential update method.
*/
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
double h_; // time step
typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
@ -73,7 +73,7 @@ public:
/**
* Implement the Discrete Euler-Poincare' equation:
*/
class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3> {
class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector6, Pose3> {
double h_; /// time step
Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ]
@ -86,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector
// This might be needed in control or system identification problems.
// We treat them as constant here, since the control inputs are to specify.
typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base;
typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,

4
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -32,9 +32,9 @@ typedef enum {
* NOTE: this approximation is insufficient for large timesteps, but is accurate
* if timesteps are small.
*/
class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> {
class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
public:
typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
typedef gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> Base;
protected:

5
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -10,11 +10,11 @@
namespace gtsam {
class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
class VelocityConstraint3 : public NoiseModelFactorN<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<double, double, double> Base;
typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */
VelocityConstraint3() {}
@ -53,6 +53,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
}

9
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
* common-mode errors and that can be partially corrected if other sensors are used
* @ingroup slam
*/
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
class BiasedGPSFactor: public NoiseModelFactorN<Pose3, Point3> {
private:
typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
typedef NoiseModelFactorN<Pose3, Point3> Base;
Point3 measured_; /** The measurement */
@ -57,8 +57,8 @@ namespace gtsam {
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"
<< keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key<2>()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
@ -96,6 +96,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

14
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -88,12 +88,12 @@ namespace gtsam {
*/
template<class POSE, class VELOCITY, class IMUBIAS>
class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
private:
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
@ -136,11 +136,11 @@ public:
/** print */
void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << "\n";
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ","
<< keyFormatter(this->template key<3>()) << ","
<< keyFormatter(this->template key<4>()) << ","
<< keyFormatter(this->template key<5>()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

12
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -87,12 +87,12 @@ namespace gtsam {
*/
template<class POSE, class VELOCITY>
class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> {
class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> {
private:
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
typedef NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
@ -136,10 +136,10 @@ public:
const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "\n";
<< keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key<3>()) << ","
<< keyFormatter(this->key<4>()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

8
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -42,12 +42,12 @@ namespace gtsam {
* T is the measurement type, by default the same
*/
template<class VALUE>
class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
class GaussMarkov1stOrderFactor: public NoiseModelFactorN<VALUE, VALUE> {
private:
typedef GaussMarkov1stOrderFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base;
typedef NoiseModelFactorN<VALUE, VALUE> Base;
double dt_;
Vector tau_;
@ -73,8 +73,8 @@ public:
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "GaussMarkov1stOrderFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n";
this->noiseModel_->print(" noise model");
}

14
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -77,12 +77,12 @@ namespace gtsam {
* vehicle
*/
template<class POSE, class VELOCITY, class IMUBIAS>
class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
private:
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector measurement_acc_;
Vector measurement_gyro_;
@ -117,11 +117,11 @@ public:
/** print */
void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << "\n";
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ","
<< keyFormatter(this->template key<3>()) << ","
<< keyFormatter(this->template key<4>()) << ","
<< keyFormatter(this->template key<5>()) << "\n";
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
std::cout << "dt: " << this->dt_ << std::endl;

8
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
* Ternary factor representing a visual measurement that includes inverse depth
*/
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -93,8 +93,8 @@ public:
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H3 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* Binary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
class InvDepthFactorVariant1: public NoiseModelFactorN<Pose3, Vector6> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector6> Base;
typedef NoiseModelFactorN<Pose3, Vector6> Base;
/// shorthand for this class
typedef InvDepthFactorVariant1 This;
@ -93,8 +93,8 @@ public:
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
<< std::endl;
return Vector::Ones(2) * 2.0 * K_->fx();
}

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/**
* Binary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
class InvDepthFactorVariant2: public NoiseModelFactorN<Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -36,7 +36,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector3> Base;
typedef NoiseModelFactorN<Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant2 This;
@ -96,8 +96,8 @@ public:
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
<< std::endl;
return Vector::Ones(2) * 2.0 * K_->fx();
}

16
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* Binary factor representing the first visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
class InvDepthFactorVariant3a: public NoiseModelFactorN<Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector3> Base;
typedef NoiseModelFactorN<Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant3a This;
@ -96,8 +96,8 @@ public:
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
<< std::endl;
return Vector::Ones(2) * 2.0 * K_->fx();
}
@ -150,7 +150,7 @@ private:
/**
* Ternary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -160,7 +160,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base;
typedef NoiseModelFactorN<Pose3, Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant3b This;
@ -222,8 +222,8 @@ public:
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key<2>())
<< std::endl;
return Vector::Ones(2) * 2.0 * K_->fx();
}

4
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -35,10 +35,10 @@ namespace gtsam {
* optimized in x1 frame in the optimization.
*/
class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
: public NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> {
: public NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base;
typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
public:
/// Constructor
LocalOrientedPlane3Factor() {}

4
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -187,8 +187,8 @@ namespace gtsam {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) <<
" moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl;
if (throwCheirality_)
throw e;
}

5
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
* @tparam VALUE is the type of variable the prior effects
*/
template<class VALUE>
class PartialPriorFactor: public NoiseModelFactor1<VALUE> {
class PartialPriorFactor: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
@ -44,7 +44,7 @@ namespace gtsam {
// Concept checks on the variable type - currently requires Lie
GTSAM_CONCEPT_LIE_TYPE(VALUE)
typedef NoiseModelFactor1<VALUE> Base;
typedef NoiseModelFactorN<VALUE> Base;
typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< Measurement on tangent space parameters, in compressed form.
@ -141,6 +141,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);

8
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -29,12 +29,12 @@ namespace gtsam {
* @ingroup slam
*/
template<class POSE>
class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {
class PoseBetweenFactor: public NoiseModelFactorN<POSE, POSE> {
private:
typedef PoseBetweenFactor<POSE> This;
typedef NoiseModelFactor2<POSE, POSE> Base;
typedef NoiseModelFactorN<POSE, POSE> Base;
POSE measured_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
@ -68,8 +68,8 @@ namespace gtsam {
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n";
measured_.print(" measured: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");

4
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -26,12 +26,12 @@ namespace gtsam {
* @ingroup slam
*/
template<class POSE>
class PosePriorFactor: public NoiseModelFactor1<POSE> {
class PosePriorFactor: public NoiseModelFactorN<POSE> {
private:
typedef PosePriorFactor<POSE> This;
typedef NoiseModelFactor1<POSE> Base;
typedef NoiseModelFactorN<POSE> Base;
POSE prior_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame

13
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -21,10 +21,10 @@ namespace gtsam {
* @ingroup slam
*/
template<typename POSE = Pose3, typename POINT = Point3>
class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
private:
typedef PoseToPointFactor This;
typedef NoiseModelFactor2<POSE, POINT> Base;
typedef NoiseModelFactorN<POSE, POINT> Base;
POINT measured_; /** the point measurement in local coordinates */
@ -47,8 +47,9 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"
std::cout << s << "PoseToPointFactor("
<< keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
@ -91,8 +92,10 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
"NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(measured_);
}

11
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
class ProjectionFactorPPP: public NoiseModelFactorN<POSE, POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -45,7 +45,7 @@ namespace gtsam {
public:
/// shorthand for base class type
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
typedef NoiseModelFactorN<POSE, POSE, LANDMARK> Base;
/// shorthand for this class
typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
@ -142,8 +142,11 @@ namespace gtsam {
if (H2) *H2 = Matrix::Zero(2,6);
if (H3) *H3 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->template key<2>())
<< " moved behind camera "
<< DefaultKeyFormatter(this->template key<1>())
<< std::endl;
if (throwCheirality_)
throw e;
}

8
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
*/
template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
: public NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> {
protected:
Point2 measured_; ///< 2D measurement
@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
public:
/// shorthand for base class type
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
typedef NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> Base;
/// shorthand for this class
typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
if (H3) *H3 = Matrix::Zero(2,3);
if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_)
throw e;
}

6
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp
View File

@ -54,9 +54,9 @@ Vector ProjectionFactorRollingShutter::evaluateError(
if (H3) *H3 = Matrix::Zero(2, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key2()) << " moved behind camera "
<< DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_) throw CheiralityException(this->key2());
<< DefaultKeyFormatter(this->key<2>()) << " moved behind camera "
<< DefaultKeyFormatter(this->key<1>()) << std::endl;
if (throwCheirality_) throw CheiralityException(this->key<2>());
}
return Vector2::Constant(2.0 * K_->fx());
}

4
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
*/
class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
: public NoiseModelFactor3<Pose3, Pose3, Point3> {
: public NoiseModelFactorN<Pose3, Pose3, Point3> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
@ -60,7 +60,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
public:
/// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
typedef NoiseModelFactorN<Pose3, Pose3, Point3> Base;
/// shorthand for this class
typedef ProjectionFactorRollingShutter This;

5
gtsam_unstable/slam/RelativeElevationFactor.h
View File

@ -25,13 +25,13 @@ namespace gtsam {
*
* TODO: enable use of a Pose3 for the target as well
*/
class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> {
class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN<Pose3, Point3> {
private:
double measured_; /** measurement */
typedef RelativeElevationFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
typedef NoiseModelFactorN<Pose3, Point3> Base;
public:
@ -66,6 +66,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);

12
gtsam_unstable/slam/TSAMFactors.h
View File

@ -26,11 +26,11 @@ namespace gtsam {
/**
* DeltaFactor: relative 2D measurement between Pose2 and Point2
*/
class DeltaFactor: public NoiseModelFactor2<Pose2, Point2> {
class DeltaFactor: public NoiseModelFactorN<Pose2, Point2> {
public:
typedef DeltaFactor This;
typedef NoiseModelFactor2<Pose2, Point2> Base;
typedef NoiseModelFactorN<Pose2, Point2> Base;
typedef boost::shared_ptr<This> shared_ptr;
private:
@ -55,11 +55,11 @@ public:
/**
* DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes
*/
class DeltaFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> {
class DeltaFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> {
public:
typedef DeltaFactorBase This;
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> Base;
typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> Base;
typedef boost::shared_ptr<This> shared_ptr;
private:
@ -110,11 +110,11 @@ public:
/**
* OdometryFactorBase: Pose2 odometry, with Basenodes
*/
class OdometryFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> {
class OdometryFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> {
public:
typedef OdometryFactorBase This;
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> Base;
typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> Base;
typedef boost::shared_ptr<This> shared_ptr;
private:

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
* @ingroup slam
*/
template<class VALUE>
class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?
class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ?
public:

12
tests/simulated2D.h
View File

@ -124,9 +124,9 @@ namespace simulated2D {
* Unary factor encoding a soft prior on a vector
*/
template<class VALUE = Point2>
class GenericPrior: public NoiseModelFactor1<VALUE> {
class GenericPrior: public NoiseModelFactorN<VALUE> {
public:
typedef NoiseModelFactor1<VALUE> Base; ///< base class
typedef NoiseModelFactorN<VALUE> Base; ///< base class
typedef GenericPrior<VALUE> This;
typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
@ -168,9 +168,9 @@ namespace simulated2D {
* Binary factor simulating "odometry" between two Vectors
*/
template<class VALUE = Point2>
class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
class GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
public:
typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class
typedef NoiseModelFactorN<VALUE, VALUE> Base; ///< base class
typedef GenericOdometry<VALUE> This;
typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
@ -214,9 +214,9 @@ namespace simulated2D {
* Binary factor simulating "measurement" between two Vectors
*/
template<class POSE, class LANDMARK>
class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> {
class GenericMeasurement: public NoiseModelFactorN<POSE, LANDMARK> {
public:
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< base class
typedef NoiseModelFactorN<POSE, LANDMARK> Base; ///< base class
typedef GenericMeasurement<POSE, LANDMARK> This;
typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
typedef POSE Pose; ///< shortcut to Pose type

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