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

support cloning (deep copy) for ISAM2

release/4.3a0
Duy-Nguyen Ta 2011-12-06 16:21:57 +00:00
parent 58ace25e33
commit e036dd460e
8 changed files with 174 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/inference/BayesTree.h
View File

@ -81,6 +81,20 @@ namespace gtsam {
Clique(const sharedConditional& conditional);
void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const {
sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_));
sharedClique newClique = newTree.addClique(newConditional, parent_clique);
if (cachedFactor_)
newClique->cachedFactor_ = cachedFactor_->clone();
else newClique->cachedFactor_ = typename FactorType::shared_ptr();
if (!parent_clique) {
newTree.root_ = newClique;
}
BOOST_FOREACH(const shared_ptr& childClique, children_) {
childClique->cloneToBayesTree(newTree, newClique);
}
}
/** print this node */
void print(const std::string& s = "") const;
@ -264,6 +278,11 @@ namespace gtsam {
/** check equality */
bool equals(const BayesTree<CONDITIONAL>& other, double tol = 1e-9) const;
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree) const {
root_->cloneToBayesTree(*newTree);
}
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.

7
gtsam/inference/Permutation.h
View File

@ -174,6 +174,13 @@ public:
/** Access the container through the permutation (const version) */
const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
/** Assignment operator for cloning in ISAM2 */
Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
permutation_ = other.permutation_;
container_ = other.container_;
return *this;
}
/** Permute this view by applying a permutation to the underlying permutation */
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }

3
gtsam/linear/GaussianFactor.h
View File

@ -88,6 +88,9 @@ namespace gtsam {
/** Return the dimension of the variable pointed to by the given key iterator */
virtual size_t getDim(const_iterator variable) const = 0;
/** Clone a factor (make a deep copy) */
virtual GaussianFactor::shared_ptr clone() const = 0;
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
* to already be inverted. This acts just as a change-of-name for each

6
gtsam/linear/HessianFactor.h
View File

@ -191,6 +191,12 @@ namespace gtsam {
/** Destructor */
virtual ~HessianFactor() {}
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new HessianFactor(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "") const;

6
gtsam/linear/JacobianFactor.h
View File

@ -136,6 +136,12 @@ namespace gtsam {
virtual ~JacobianFactor() {}
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new JacobianFactor(*this)));
}
// Implementing Testable interface
virtual void print(const std::string& s = "") const;
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

6
gtsam/nonlinear/GaussianISAM2.h
View File

@ -27,12 +27,18 @@ namespace gtsam {
*/
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
public:
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
Base::cloneTo(isam2);
}
};
// optimize the BayesTree, starting from the root

23
gtsam/nonlinear/ISAM2.h
View File

@ -167,6 +167,27 @@ public:
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
ISAM2();
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
Base::cloneTo(bayesTree);
newISAM2->theta_ = theta_;
newISAM2->variableIndex_ = variableIndex_;
newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
newISAM2->delta_ = delta_;
newISAM2->nonlinearFactors_ = nonlinearFactors_;
newISAM2->ordering_ = ordering_;
newISAM2->params_ = params_;
#ifndef NDEBUG
newISAM2->lastRelinVariables_ = lastRelinVariables_;
#endif
newISAM2->lastAffectedVariableCount = lastAffectedVariableCount;
newISAM2->lastAffectedFactorCount = lastAffectedFactorCount;
newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount;
newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount;
newISAM2->lastBacksubVariableCount = lastBacksubVariableCount;
newISAM2->lastNnzTop = lastNnzTop;
}
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///< List of Clique typedef from base class
@ -235,6 +256,8 @@ public:
size_t lastBacksubVariableCount;
size_t lastNnzTop;
ISAM2Params params() const { return params_; }
//@}
private:

104
tests/testGaussianISAM2.cpp
View File

@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution)
EXPECT(isam_check(fullgraph, fullinit, isam));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, clone) {
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(0.001, 0.0, 0, false, true));
planarSLAM::Values fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
EXPECT(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// CLONING...
boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2
= boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>());
isam.cloneTo(isam2);
CHECK(assert_equal(isam, *isam2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */