Merge branch 'master' into wrap_mods_local
Alex Cunningham
parent
5d58dbd512
commit
e337ab8b1d
|
|
@ -31,7 +31,7 @@ namespace visualSLAM {
|
|||
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
|
||||
if (Z.cols() != J.size()) throw std::invalid_argument(
|
||||
"insertBackProjections: J and Z must have same number of entries");
|
||||
for(size_t k=0;k<Z.cols();k++) {
|
||||
for(int k=0;k<Z.cols();k++) {
|
||||
Point2 p(Z(0,k),Z(1,k));
|
||||
Point3 P = camera.backproject(p, depth);
|
||||
insertPoint(J(k), P);
|
||||
|
|
@ -82,7 +82,7 @@ namespace visualSLAM {
|
|||
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
|
||||
if (Z.cols() != J.size()) throw std::invalid_argument(
|
||||
"addMeasurements: J and Z must have same number of entries");
|
||||
for (size_t k = 0; k < Z.cols(); k++)
|
||||
for (int k = 0; k < Z.cols(); k++)
|
||||
addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,148 @@
|
|||
/**
|
||||
* @file LinearContainerFactor.cpp
|
||||
*
|
||||
* @date Jul 6, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
|
||||
Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
|
||||
rekeyFactor(invOrdering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
|
||||
BOOST_FOREACH(Index& idx, factor_->keys()) {
|
||||
Key fullKey = invOrdering.find(idx)->second;
|
||||
idx = fullKey;
|
||||
keys_.push_back(fullKey);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const JacobianFactor& factor, const Ordering& ordering)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const HessianFactor& factor, const Ordering& ordering)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const GaussianFactor::shared_ptr& factor, const Ordering& ordering)
|
||||
: factor_(factor->clone()) {
|
||||
rekeyFactor(ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const GaussianFactor::shared_ptr& factor)
|
||||
: factor_(factor->clone())
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(inverted_ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(inverted_ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const GaussianFactor::shared_ptr& factor,
|
||||
const Ordering::InvertedMap& ordering)
|
||||
: factor_(factor->clone()) {
|
||||
rekeyFactor(ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
Base::print(s+"LinearContainerFactor", keyFormatter);
|
||||
if (factor_)
|
||||
factor_->print(" Stored Factor", keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
|
||||
const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f);
|
||||
return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double LinearContainerFactor::error(const Values& c) const {
|
||||
// VectorValues vecvalues;
|
||||
// // FIXME: add values correctly here
|
||||
// return factor_.error(vecvalues);
|
||||
return 0; // FIXME: placeholder
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
size_t LinearContainerFactor::dim() const {
|
||||
if (isJacobian())
|
||||
return toJacobian()->get_model()->dim();
|
||||
else
|
||||
return 1; // Hessians don't have true dimension
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||
// clone factor
|
||||
boost::shared_ptr<GaussianFactor> result = factor_->clone();
|
||||
|
||||
// rekey
|
||||
BOOST_FOREACH(Index& key, result->keys())
|
||||
key = ordering[key];
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LinearContainerFactor::isJacobian() const {
|
||||
return boost::shared_dynamic_cast<JacobianFactor>(factor_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const {
|
||||
return boost::shared_dynamic_cast<JacobianFactor>(factor_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
|
||||
return boost::shared_dynamic_cast<HessianFactor>(factor_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const {
|
||||
GaussianFactor::shared_ptr result = factor_->negate();
|
||||
BOOST_FOREACH(Key& key, result->keys())
|
||||
key = ordering[key];
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
/**
|
||||
* @file LinearContainerFactor.h
|
||||
*
|
||||
* @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph
|
||||
*
|
||||
* @date Jul 6, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Dummy version of a generic linear factor to be injected into a nonlinear factor graph
|
||||
*/
|
||||
class LinearContainerFactor : public NonlinearFactor {
|
||||
protected:
|
||||
|
||||
GaussianFactor::shared_ptr factor_;
|
||||
|
||||
public:
|
||||
|
||||
/** Primary constructor: store a linear factor and decode the ordering */
|
||||
LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering);
|
||||
|
||||
/** Primary constructor: store a linear factor and decode the ordering */
|
||||
LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering);
|
||||
|
||||
/** Constructor from shared_ptr */
|
||||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
|
||||
|
||||
/** Constructor from re-keyed factor: all indices assumed replaced with Key */
|
||||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor);
|
||||
|
||||
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||
LinearContainerFactor(const JacobianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering);
|
||||
|
||||
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||
LinearContainerFactor(const HessianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering);
|
||||
|
||||
/** Constructor from shared_ptr with inverted ordering*/
|
||||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
|
||||
const Ordering::InvertedMap& ordering);
|
||||
|
||||
// Access
|
||||
|
||||
const GaussianFactor::shared_ptr& factor() const { return factor_; }
|
||||
|
||||
// Testable
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
|
||||
|
||||
// NonlinearFactor
|
||||
|
||||
/**
|
||||
* Calculate the error of the factor: uses the underlying linear factor to compute ordering
|
||||
*/
|
||||
double error(const Values& c) const;
|
||||
|
||||
/** get the dimension of the factor: rows of linear factor */
|
||||
size_t dim() const;
|
||||
|
||||
/** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
linearize(const Values& c, const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Creates an anti-factor directly and performs rekeying due to ordering
|
||||
*/
|
||||
GaussianFactor::shared_ptr negate(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
|
||||
* for subclasses
|
||||
*
|
||||
* Clones the underlying linear factor
|
||||
*/
|
||||
NonlinearFactor::shared_ptr clone() const {
|
||||
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
|
||||
}
|
||||
|
||||
// casting syntactic sugar
|
||||
|
||||
/**
|
||||
* Simple check whether this is a Jacobian or Hessian factor
|
||||
*/
|
||||
bool isJacobian() const;
|
||||
|
||||
/** Casts to JacobianFactor */
|
||||
JacobianFactor::shared_ptr toJacobian() const;
|
||||
|
||||
/** Casts to HessianFactor */
|
||||
HessianFactor::shared_ptr toHessian() const;
|
||||
|
||||
protected:
|
||||
void rekeyFactor(const Ordering& ordering);
|
||||
void rekeyFactor(const Ordering::InvertedMap& invOrdering);
|
||||
|
||||
}; // \class LinearContainerFactor
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
/**
|
||||
* @file testLinearContainerFactor.cpp
|
||||
*
|
||||
* @date Jul 6, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
|
||||
const double tol = 1e-5;
|
||||
|
||||
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
|
||||
|
||||
Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
|
||||
Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
|
||||
|
||||
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
|
||||
|
||||
JacobianFactor expLinFactor1(
|
||||
initOrdering[l1],
|
||||
Matrix_(2,2,
|
||||
2.74222, -0.0067457,
|
||||
0.0, 2.63624),
|
||||
initOrdering[l2],
|
||||
Matrix_(2,2,
|
||||
-0.0455167, -0.0443573,
|
||||
-0.0222154, -0.102489),
|
||||
Vector_(2, 0.0277052,
|
||||
-0.0533393),
|
||||
diag_model2);
|
||||
|
||||
LinearContainerFactor actFactor1(expLinFactor1, initOrdering);
|
||||
Values values;
|
||||
values.insert(l1, landmark1);
|
||||
values.insert(l2, landmark2);
|
||||
values.insert(x1, poseA1);
|
||||
values.insert(x2, poseA2);
|
||||
|
||||
// Check reconstruction from same ordering
|
||||
GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering);
|
||||
EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol));
|
||||
|
||||
// Check reconstruction from new ordering
|
||||
Ordering newOrdering; newOrdering += x1, l1, x2, l2;
|
||||
GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering);
|
||||
|
||||
JacobianFactor expLinFactor2(
|
||||
newOrdering[l1],
|
||||
Matrix_(2,2,
|
||||
2.74222, -0.0067457,
|
||||
0.0, 2.63624),
|
||||
newOrdering[l2],
|
||||
Matrix_(2,2,
|
||||
-0.0455167, -0.0443573,
|
||||
-0.0222154, -0.102489),
|
||||
Vector_(2, 0.0277052,
|
||||
-0.0533393),
|
||||
diag_model2);
|
||||
|
||||
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLinearContainerFactor, generic_hessian_factor ) {
|
||||
Matrix G11 = Matrix_(1,1, 1.0);
|
||||
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
|
||||
|
||||
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
|
||||
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
|
||||
|
||||
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
|
||||
|
||||
Vector g1 = Vector_(1, -7.0);
|
||||
Vector g2 = Vector_(2, -8.0, -9.0);
|
||||
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
|
||||
|
||||
double f = 10.0;
|
||||
|
||||
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
|
||||
HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
|
||||
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||
|
||||
Values values;
|
||||
values.insert(l1, landmark1);
|
||||
values.insert(l2, landmark2);
|
||||
values.insert(x1, poseA1);
|
||||
values.insert(x2, poseA2);
|
||||
|
||||
LinearContainerFactor actFactor(initFactor, initOrdering);
|
||||
GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
|
||||
EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
|
||||
|
||||
Ordering newOrdering; newOrdering += l1, x1, x2, l2;
|
||||
HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
|
||||
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||
GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
|
||||
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -431,7 +431,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
|
|||
bool totalGradOk = assert_equal(expectedGradient, actualGradient);
|
||||
EXPECT(totalGradOk);
|
||||
|
||||
return nodeGradientsOk && expectedGradOk && totalGradOk;
|
||||
return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue