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

addConstraint

release/4.3a0
Frank Dellaert 2010-01-10 19:25:19 +00:00
parent d11b2e6bd5
commit 64eca2d550
5 changed files with 165 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
cpp/NonlinearEquality.h
View File

@ -12,6 +12,17 @@
namespace gtsam {
/**
* Template default compare function that assumes Congig.get yields a testable T
*/
template<class Config, class T>
bool compare(const std::string& key, const Config& feasible, const Config& input) {
const T& t1 = feasible.get(key);
const T& t2 = input.get(key);
return t1.equals(t2);
}
/**
* An equality factor that forces either one variable to a constant,
* or a set of variables to be equal to each other.
@ -41,19 +52,28 @@ public:
* @param input is the config to be tested for feasibility
* @return true if the linearization point is feasible
*/
bool (*compare_)(const std::string& key, const Config& feasible, const Config& input);
bool (*compare_)(const std::string& key, const Config& feasible,
const Config& input);
/** Constructor */
NonlinearEquality(const std::string& key,
const Config& feasible,
size_t dim,
bool (*compare)(const std::string& key,
const Config& feasible,
const Config& input))
: key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
NonlinearEquality(const std::string& key, const Config& feasible,
size_t dim, bool(*compare)(const std::string& key,
const Config& feasible, const Config& input)) :
key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
}
/**
* Constructor with default compare
* Needs class T to have dim()
* and Config to have insert and get
*/
template <class T>
NonlinearEquality(const std::string& key, const T& feasible) :
key_(key), dim_(dim(feasible)), compare_(compare<Config,T>) {
feasible_.insert(key,feasible);
}
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
feasible_.print("Feasible Point");
@ -62,7 +82,8 @@ public:
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const NonlinearEquality<Config>* p = dynamic_cast<const NonlinearEquality<Config>*> (&f);
const NonlinearEquality<Config>* p =
dynamic_cast<const NonlinearEquality<Config>*> (&f);
if (p == NULL) return false;
if (key_ != p->key_) return false;
if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
@ -80,13 +101,15 @@ public:
/** linearize a nonlinear constraint into a linear constraint */
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
if (!compare_(key_, feasible_, c)) {
throw std::invalid_argument("Linearization point not feasible for " + key_ + "!");
throw std::invalid_argument("Linearization point not feasible for "
+ key_ + "!");
} else {
GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), zero(dim_), 0.0));
GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_),
zero(dim_), 0.0));
return ret;
}
}
};
}; // NonlinearEquality
}
} // namespace gtsam

16
cpp/Pose2Graph.h
View File

@ -8,6 +8,7 @@
#pragma once
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
#include "Pose2Factor.h"
#include "Pose2Config.h"
@ -21,7 +22,8 @@ class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config> {
public:
/** default constructor is empty graph */
Pose2Graph() {}
Pose2Graph() {
}
/**
* equals
@ -36,11 +38,21 @@ public:
push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
}
/**
* Add an equality constraint on a pose
* @param key of pose
* @param pose which pose to constrain it to
*/
inline void addConstraint(const std::string& key, const Pose2& pose = Pose2()) {
push_back(sharedFactor(new NonlinearEquality<Pose2Config>(key, pose)));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {}
void serialize(Archive & ar, const unsigned int version) {
}
};
} // namespace gtsam

16
cpp/Pose3Graph.h
View File

@ -8,6 +8,7 @@
#pragma once
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
#include "Pose3Factor.h"
#include "Pose3Config.h"
@ -21,7 +22,8 @@ class Pose3Graph : public gtsam::NonlinearFactorGraph<Pose3Config> {
public:
/** default constructor is empty graph */
Pose3Graph() {}
Pose3Graph() {
}
/**
* equals
@ -36,11 +38,21 @@ public:
push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
}
/**
* Add an equality constraint on a pose
* @param key of pose
* @param pose which pose to constrain it to
*/
inline void addConstraint(const std::string& key, const Pose3& pose = Pose3()) {
push_back(sharedFactor(new NonlinearEquality<Pose3Config> (key, pose)));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {}
void serialize(Archive & ar, const unsigned int version) {
}
};
} // namespace gtsam

19
cpp/testPose2Graph.cpp
View File

@ -11,7 +11,6 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
#include "Ordering.h"
#include "Pose2Config.h"
#include "Pose2Graph.h"
@ -81,22 +80,12 @@ TEST( Pose2Graph, linerization )
CHECK(assert_equal(lfg_expected, lfg_linearized));
}
/* ************************************************************************* */
bool poseCompare(const std::string& key,
const gtsam::Pose2Config& feasible,
const gtsam::Pose2Config& input) {
return feasible.get(key).equals(input.get(key));
}
/* ************************************************************************* */
TEST(Pose2Graph, optimize) {
// create a Pose graph with one equality constraint and one measurement
Pose2Graph fg;
Pose2Config feasible;
feasible.insert("p0", Pose2(0,0,0));
fg.push_back(Pose2Graph::sharedFactor(
new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
fg.addConstraint("p0", Pose2(0,0,0));
fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
// Create initial config
@ -118,7 +107,6 @@ TEST(Pose2Graph, optimize) {
expected.insert("p0", Pose2(0,0,0));
expected.insert("p1", Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.config()));
}
/* ************************************************************************* */
@ -131,10 +119,7 @@ TEST(Pose2Graph, optimizeCircle) {
// create a Pose graph with one equality constraint and one measurement
Pose2Graph fg;
Pose2Config feasible;
feasible.insert("p0", p0);
fg.push_back(Pose2Graph::sharedFactor(
new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
fg.addConstraint("p0", p0);
Pose2 delta = between(p0,p1);
fg.add("p0", "p1", delta, covariance);
fg.add("p1", "p2", delta, covariance);

12
cpp/testPose3Graph.cpp
View File

@ -23,13 +23,6 @@ using namespace gtsam;
// common measurement covariance
static Matrix covariance = eye(6);
/* ************************************************************************* */
bool poseCompare(const std::string& key,
const gtsam::Pose3Config& feasible,
const gtsam::Pose3Config& input) {
return feasible.get(key).equals(input.get(key));
}
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose3Graph, optimizeCircle) {
@ -40,10 +33,7 @@ TEST(Pose3Graph, optimizeCircle) {
// create a Pose graph with one equality constraint and one measurement
Pose3Graph fg;
Pose3Config feasible;
feasible.insert("p0", p0);
fg.push_back(Pose3Graph::sharedFactor(
new NonlinearEquality<Pose3Config>("p0", feasible, dim(Pose3()), poseCompare)));
fg.addConstraint("p0", p0);
Pose3 delta = between(p0,p1);
fg.add("p0", "p1", delta, covariance);
fg.add("p1", "p2", delta, covariance);