Moved DummyFactor to gtsam_unstable from MastSLAM - allows for adding sufficient connectivity for solvers to operate
parent
7fcd06bb4f
commit
f272a07e29
|
@ -199,4 +199,9 @@ virtual class RelativeElevationFactor: gtsam::NonlinearFactor {
|
|||
void print(string s) const;
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/slam/DummyFactor.h>
|
||||
virtual class DummyFactor : gtsam::NonlinearFactor {
|
||||
DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2);
|
||||
};
|
||||
|
||||
} //\namespace gtsam
|
||||
|
|
|
@ -0,0 +1,64 @@
|
|||
/**
|
||||
* @file DummyFactor.cpp
|
||||
*
|
||||
* @date Sep 10, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/slam/DummyFactor.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
|
||||
: NonlinearFactor(key1, key2)
|
||||
{
|
||||
dims_.push_back(dim1);
|
||||
dims_.push_back(dim2);
|
||||
if (dim1 > dim2)
|
||||
rowDim_ = dim1;
|
||||
else
|
||||
rowDim_ = dim2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << " DummyFactor dim = " << rowDim_ << ", keys = { ";
|
||||
BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
|
||||
std::cout << "}" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
|
||||
const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f);
|
||||
return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(c))
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
// Fill in terms with zero matrices
|
||||
std::vector<std::pair<Index, Matrix> > terms(this->size());
|
||||
for(size_t j=0; j<this->size(); ++j) {
|
||||
terms[j].first = ordering[this->keys()[j]];
|
||||
terms[j].second = zeros(rowDim_, dims_[j]);
|
||||
}
|
||||
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(terms, zero(rowDim_), model));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
/**
|
||||
* @file DummyFactor.h
|
||||
*
|
||||
* @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected.
|
||||
*
|
||||
* @date Sep 10, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class DummyFactor : public NonlinearFactor {
|
||||
protected:
|
||||
|
||||
// Store the dimensions of the variables and the dimension of the full system
|
||||
std::vector<size_t> dims_;
|
||||
size_t rowDim_; ///< choose dimension for the rows
|
||||
|
||||
public:
|
||||
|
||||
/** Default constructor: don't use directly */
|
||||
DummyFactor() : rowDim_(1) { }
|
||||
|
||||
/** standard binary constructor */
|
||||
DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2);
|
||||
|
||||
virtual ~DummyFactor() {}
|
||||
|
||||
// testable
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
|
||||
|
||||
// access
|
||||
|
||||
const std::vector<size_t>& dims() const { return dims_; }
|
||||
|
||||
// factor interface
|
||||
|
||||
/**
|
||||
* Calculate the error of the factor - zero for dummy factors
|
||||
*/
|
||||
virtual double error(const Values& c) const { return 0.0; }
|
||||
|
||||
/** get the dimension of the factor (number of rows on linearization) */
|
||||
virtual size_t dim() const { return rowDim_; }
|
||||
|
||||
/** linearize to a GaussianFactor */
|
||||
virtual boost::shared_ptr<GaussianFactor>
|
||||
linearize(const Values& c, const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
|
||||
* for subclasses
|
||||
*
|
||||
* By default, throws exception if subclass does not implement the function.
|
||||
*/
|
||||
virtual NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<NonlinearFactor>(
|
||||
NonlinearFactor::shared_ptr(new DummyFactor(*this)));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/**
|
||||
* @file testDummyFactor.cpp
|
||||
*
|
||||
* @brief A simple dummy nonlinear factor that can be used to enforce connectivity
|
||||
* without adding any real information
|
||||
*
|
||||
* @date Sep 10, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
#include <gtsam_unstable/slam/DummyFactor.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol = 1e-9;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testDummyFactor, basics ) {
|
||||
|
||||
gtsam::Key key1 = 7, key2 = 9;
|
||||
size_t dim1 = 3, dim2 = 3;
|
||||
DummyFactor dummyfactor(key1, dim1, key2, dim2);
|
||||
|
||||
// verify contents
|
||||
LONGS_EQUAL(2, dummyfactor.size());
|
||||
EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]);
|
||||
EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]);
|
||||
|
||||
LONGS_EQUAL(2, dummyfactor.dims().size());
|
||||
EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]);
|
||||
EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]);
|
||||
|
||||
Values c;
|
||||
c.insert(key1, Point3(1.0, 2.0, 3.0));
|
||||
c.insert(key2, Point3(4.0, 5.0, 6.0));
|
||||
|
||||
// errors - all zeros
|
||||
DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol);
|
||||
|
||||
Ordering ordering;
|
||||
ordering += key1, key2;
|
||||
|
||||
// linearization: all zeros
|
||||
GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering);
|
||||
CHECK(actLinearization);
|
||||
noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
|
||||
GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
|
||||
ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3));
|
||||
EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
Loading…
Reference in New Issue