BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
Frank Dellaert
parent
e5fe4a69f7
commit
5dc237eeea
|
|
@ -0,0 +1,91 @@
|
|||
/**
|
||||
* @file BetweenFactor.h
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <ostream>
|
||||
|
||||
#include "NonlinearFactor.h"
|
||||
#include "GaussianFactor.h"
|
||||
#include "Lie.h"
|
||||
#include "Matrix.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A class for a measurement predicted by "between(config[key1],config[key2])"
|
||||
* T is the Lie group type, Config where the T's are gotten from
|
||||
*/
|
||||
template<class T, class Config>
|
||||
class BetweenFactor: public NonlinearFactor<Config> {
|
||||
|
||||
private:
|
||||
|
||||
T measured_; /** The measurement */
|
||||
std::string key1_, key2_; /** The keys of the two poses, order matters */
|
||||
std::list<std::string> keys_; /** The keys as a list */
|
||||
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
|
||||
|
||||
/** Constructor */
|
||||
BetweenFactor(const std::string& key1, const std::string& key2,
|
||||
const T& measured, const Matrix& measurement_covariance) :
|
||||
key1_(key1), key2_(key2), measured_(measured) {
|
||||
square_root_inverse_covariance_ = inverse_square_root(
|
||||
measurement_covariance);
|
||||
keys_.push_back(key1);
|
||||
keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
void print(const std::string& name) const {
|
||||
std::cout << name << std::endl;
|
||||
std::cout << "Factor " << std::endl;
|
||||
std::cout << "key1 " << key1_ << std::endl;
|
||||
std::cout << "key2 " << key2_ << std::endl;
|
||||
measured_.print("measured ");
|
||||
gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
|
||||
return key1_ == expected.key1_ && key2_ == expected.key2_
|
||||
&& measured_.equals(expected.measured_, tol);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector error_vector(const Config& config) const {
|
||||
//z-h
|
||||
T p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
return -logmap(between(p1, p2), measured_);
|
||||
}
|
||||
|
||||
/** keys as a list */
|
||||
inline std::list<std::string> keys() const { return keys_;}
|
||||
|
||||
/** number of variables attached to this factor */
|
||||
inline std::size_t size() const { return 2;}
|
||||
|
||||
/** linearize */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
|
||||
T p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
Vector b = -logmap(between(p1, p2), measured_);
|
||||
Matrix H1 = Dbetween1(p1, p2);
|
||||
Matrix H2 = Dbetween2(p1, p2);
|
||||
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_,
|
||||
square_root_inverse_covariance_ * H1, key2_,
|
||||
square_root_inverse_covariance_ * H2, b, 1.0));
|
||||
return linearized;
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -193,8 +193,8 @@ testSimulated3D_LDADD = libgtsam.la
|
|||
check_PROGRAMS += testSimulated3D
|
||||
|
||||
# Pose constraints
|
||||
headers += Pose2Factor.h Pose2Prior.h
|
||||
sources += Pose2Config.cpp Pose2Graph.cpp Pose3Factor.cpp
|
||||
headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h
|
||||
sources += Pose2Config.cpp Pose2Graph.cpp
|
||||
check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor
|
||||
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
|
||||
testPose2Factor_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -2,71 +2,16 @@
|
|||
* @file Pose2Factor.H
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include "NonlinearFactor.h"
|
||||
#include "GaussianFactor.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "BetweenFactor.h"
|
||||
#include "Pose2.h"
|
||||
#include "Matrix.h"
|
||||
#include "Pose2Config.h"
|
||||
#include "ostream"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Pose2Factor : public NonlinearFactor<Pose2Config> {
|
||||
private:
|
||||
std::string key1_, key2_; /** The keys of the two poses, order matters */
|
||||
Pose2 measured_;
|
||||
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
|
||||
std::list<std::string> keys_;
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Pose2Factor> shared_ptr; // shorthand for a smart pointer to a factor
|
||||
|
||||
Pose2Factor(const std::string& key1, const std::string& key2,
|
||||
const Pose2& measured, const Matrix& measurement_covariance) :
|
||||
key1_(key1),key2_(key2),measured_(measured) {
|
||||
square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
|
||||
keys_.push_back(key1);
|
||||
keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
void print(const std::string& name) const {
|
||||
std::cout << name << std::endl;
|
||||
std::cout << "Factor "<< std::endl;
|
||||
std::cout << "key1 "<< key1_<<std::endl;
|
||||
std::cout << "key2 "<< key2_<<std::endl;
|
||||
measured_.print("measured ");
|
||||
gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
|
||||
}
|
||||
bool equals(const NonlinearFactor<Pose2Config>& expected, double tol) const {return false;}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
Vector error_vector(const Pose2Config& config) const {
|
||||
//z-h
|
||||
Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
return -logmap(between(p1,p2), measured_);
|
||||
}
|
||||
|
||||
std::list<std::string> keys() const { return keys_; }
|
||||
std::size_t size() const { return keys_.size(); }
|
||||
|
||||
/** linearize */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
|
||||
Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
Vector b = -logmap(between(p1,p2), measured_);
|
||||
Matrix H1 = Dbetween1(p1,p2);
|
||||
Matrix H2 = Dbetween2(p1,p2);
|
||||
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
|
||||
key1_, square_root_inverse_covariance_ * H1,
|
||||
key2_, square_root_inverse_covariance_ * H2,
|
||||
b, 1.0));
|
||||
return linearized;
|
||||
}
|
||||
};
|
||||
/** This is just a typedef now */
|
||||
typedef BetweenFactor<Pose2, Pose2Config> Pose2Factor;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,86 +0,0 @@
|
|||
/**
|
||||
* @file UrbanOdometryFactor.cpp
|
||||
* @brief A non-linear factor specialized to the Visual SLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include "Pose3Factor.h"
|
||||
|
||||
using namespace std;
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* *
|
||||
Pose3Factor::Pose3Factor() {
|
||||
/// Arbitrary values
|
||||
robotPoseNumber_ = 1;
|
||||
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
}
|
||||
/* ************************************************************************* *
|
||||
Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln)
|
||||
: NonlinearFactor<Pose3Config>(z.vector(), sigma)
|
||||
{
|
||||
robotPoseNumber_ = cn;
|
||||
landmarkNumber_ = ln;
|
||||
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||
landmarkName_ = symbol('l',landmarkNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
keys_.push_back(landmarkName_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
void Pose3Factor::print(const std::string& s) const {
|
||||
printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str());
|
||||
gtsam::print(this->z_, s+".z");
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
bool Pose3Factor::equals(const Pose3Factor& p, double tol) const {
|
||||
if (&p == NULL) return false;
|
||||
if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
|
||||
if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
// TODO Implement transformPoint2_from
|
||||
// the difference here that we have a 2d point in a 3D coordinate
|
||||
Vector Pose3Factor::predict(const Pose3Config& c) const {
|
||||
Pose3 pose = c.cameraPose(robotPoseNumber_);
|
||||
Point2 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector();
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Vector Pose3Factor::error_vector(const Pose3Config& c) const {
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
Point2 h = predict(c);
|
||||
return (this->z_ - h);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
GaussianFactor::shared_ptr Pose3Factor::linearize(const Pose3Config& c) const
|
||||
{
|
||||
// get arguments from config
|
||||
Pose3 pose = c.cameraPose(robotPoseNumber_);
|
||||
Point3 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
|
||||
SimpleCamera camera(*K_,pose);
|
||||
|
||||
// Jacobians
|
||||
Matrix Dh1 = Dproject_pose(camera, landmark);
|
||||
Matrix Dh2 = Dproject_point(camera, landmark);
|
||||
|
||||
// Right-hand-side b = (z - h(x))
|
||||
Vector h = project(camera, landmark).vector();
|
||||
Vector b = this->z_ - h;
|
||||
|
||||
// Make new linearfactor, divide by sigma
|
||||
GaussianFactor::shared_ptr
|
||||
p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
|
||||
return p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,88 +1,29 @@
|
|||
/**
|
||||
* @file Pose3Factor.h
|
||||
* @brief A Nonlinear Factor, specialized for Urban application
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
* @file Pose3Factor.H
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include "NonlinearFactor.h"
|
||||
#include "GaussianFactor.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "BetweenFactor.h"
|
||||
#include "Pose3.h"
|
||||
#include "Matrix.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef VectorConfig Config;
|
||||
|
||||
class Pose3Factor: public NonlinearFactor<Config> {
|
||||
private:
|
||||
std::string key1_, key2_; /** The keys of the two poses, order matters */
|
||||
Pose3 measured_;
|
||||
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
|
||||
class Pose3Config: public std::map<std::string, Pose3> {
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Pose3Factor> shared_ptr; // shorthand for a smart pointer to a factor
|
||||
|
||||
Pose3Factor(const std::string& key1, const std::string& key2,
|
||||
const Pose3& measured, const Matrix& measurement_covariance) :
|
||||
key1_(key1), key2_(key2), measured_(measured) {
|
||||
square_root_inverse_covariance_ = inverse_square_root(
|
||||
measurement_covariance);
|
||||
const Pose3& get(std::string key) const {
|
||||
const_iterator it = find(key);
|
||||
if (it == end()) throw std::invalid_argument("invalid key");
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
void print(const std::string& name) const {
|
||||
std::cout << name << std::endl;
|
||||
std::cout << "Factor " << std::endl;
|
||||
std::cout << "key1 " << key1_ << std::endl;
|
||||
std::cout << "key2 " << key2_ << std::endl;
|
||||
measured_.print("measured ");
|
||||
gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
|
||||
}
|
||||
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
Vector error_vector(const Config& config) const {
|
||||
//z-h
|
||||
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
// todo: removed vector() from Pose3, so emulating it here! This is incorrect!
|
||||
Pose3 betw = between(p1,p2);
|
||||
Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
|
||||
Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
|
||||
return concatVectors(2, &r_diff, &t_diff);
|
||||
}
|
||||
|
||||
std::list<std::string> keys() const {
|
||||
std::list<std::string> l;
|
||||
return l;
|
||||
}
|
||||
std::size_t size() const {
|
||||
return 2;
|
||||
}
|
||||
|
||||
/** linearize */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
|
||||
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||
Pose3 betw = between(p1,p2);
|
||||
// todo: removed vector() from Pose3, so emulating it here! This is incorrect!
|
||||
Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
|
||||
Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
|
||||
Vector b = concatVectors(2, &r_diff, &t_diff);
|
||||
Matrix H1 = Dbetween1(p1, p2);
|
||||
Matrix H2 = Dbetween2(p1, p2);
|
||||
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_,
|
||||
square_root_inverse_covariance_ * H1, key2_,
|
||||
square_root_inverse_covariance_ * H2, b, 1.0));
|
||||
return linearized;
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
/** This is just a typedef now */
|
||||
typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -11,8 +11,12 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3Factor, constructor )
|
||||
{
|
||||
Pose3 measured;
|
||||
Matrix measurement_covariance;
|
||||
Pose3Factor("x1", "x2", measured, measurement_covariance);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue