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

Fixed deep copy in iSAM2 and added unit test to reproduce problem

release/4.3a0
Richard Roberts 2012-04-06 18:56:07 +00:00
parent da1c65b394
commit 45f2101f48
6 changed files with 216 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/inference/BayesTree-inl.h
View File

@ -315,6 +315,20 @@ namespace gtsam {
root_ = new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
*this = other;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) {
this->clear();
other.cloneTo(*this);
return *this;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::print(const string& s) const {
@ -568,7 +582,16 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(
This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree.addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
}
/// namespace gtsam

25
gtsam/inference/BayesTree.h
View File

@ -54,6 +54,7 @@ namespace gtsam {
public:
typedef BayesTree<CONDITIONAL, CLIQUE> This;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
@ -146,6 +147,12 @@ namespace gtsam {
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
/** Copy constructor */
BayesTree(const This& other);
/** Assignment operator */
This& operator=(const This& other);
/// @}
/// @name Advanced Constructors
/// @{
@ -273,21 +280,15 @@ namespace gtsam {
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
///TODO: comment
void cloneTo(shared_ptr& newTree) const {
private:
/** deep copy to another tree */
void cloneTo(This& newTree) const {
cloneTo(newTree, root(), sharedClique());
}
private:
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree->addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
/** deep copy to another tree */
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
/** Serialization function */
friend class boost::serialization::access;

15
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -134,7 +134,9 @@ namespace gtsam {
*/
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
///TODO: comment
/** Returns a new clique containing a copy of the conditional but without
* the parent and child clique pointers.
*/
derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
/** Permute the variables in the whole subtree rooted at this clique */
@ -165,6 +167,17 @@ namespace gtsam {
void assertInvariants() const;
private:
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
BayesTreeCliqueBase(const This& other) { assert(false); }
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
This& operator=(const This& other) { assert(false); return *this; }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

43
gtsam/nonlinear/ISAM2.cpp
View File

@ -42,7 +42,6 @@ static const double batchThreshold = 0.65;
ISAM2::ISAM2(const ISAM2Params& params):
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
@ -51,11 +50,51 @@ ISAM2::ISAM2(const ISAM2Params& params):
ISAM2::ISAM2():
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), deltaUptodate_(true) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2& other):
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) {
*this = other;
}
/* ************************************************************************* */
ISAM2& ISAM2::operator=(const ISAM2& rhs) {
// Copy BayesTree
this->Base::operator=(rhs);
// Copy our variables
// When we have Permuted<...>, it is only necessary to copy this permuted
// view and not the original, because copying the permuted view automatically
// copies the original.
theta_ = rhs.theta_;
variableIndex_ = rhs.variableIndex_;
delta_ = rhs.delta_;
deltaNewton_ = rhs.deltaNewton_;
RgProd_ = rhs.RgProd_;
deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_;
deltaUptodate_ = rhs.deltaUptodate_;
deltaReplacedMask_ = rhs.deltaReplacedMask_;
nonlinearFactors_ = rhs.nonlinearFactors_;
ordering_ = rhs.ordering_;
params_ = rhs.params_;
doglegDelta_ = rhs.doglegDelta_;
#ifndef NDEBUG
lastRelinVariables_ = rhs.lastRelinVariables_;
#endif
lastAffectedVariableCount = rhs.lastAffectedVariableCount;
lastAffectedFactorCount = rhs.lastAffectedFactorCount;
lastAffectedCliqueCount = rhs.lastAffectedCliqueCount;
lastAffectedMarkedCount = rhs.lastAffectedMarkedCount;
lastBacksubVariableCount = rhs.lastBacksubVariableCount;
lastNnzTop = rhs.lastNnzTop;
return *this;
}
/* ************************************************************************* */
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
static const bool debug = false;

40
gtsam/nonlinear/ISAM2.h
View File

@ -329,6 +329,10 @@ protected:
* delta will always be updated if necessary when requested with getDelta()
* or calculateEstimate().
*
* This does not need to be permuted because any change in variable ordering
* that would cause a permutation will also mark variables as needing to be
* updated in this mask.
*
* This is \c mutable because it is used internally to not update delta_
* until it is needed.
*/
@ -356,6 +360,7 @@ private:
public:
typedef ISAM2 This; ///< This class
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
/** Create an empty ISAM2 instance */
@ -364,39 +369,16 @@ public:
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
ISAM2();
/** Copy constructor */
ISAM2(const ISAM2& other);
/** Assignment operator */
ISAM2& operator=(const ISAM2& rhs);
typedef Base::Clique Clique; ///< A clique
typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique
typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
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->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_;
newISAM2->deltaNewton_ = deltaNewton_;
newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_;
newISAM2->RgProd_ = RgProd_;
newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_;
newISAM2->deltaUptodate_ = deltaUptodate_;
newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
newISAM2->nonlinearFactors_ = nonlinearFactors_;
newISAM2->ordering_ = ordering_;
newISAM2->params_ = params_;
newISAM2->doglegDelta_ = doglegDelta_;
#ifndef NDEBUG
newISAM2->lastRelinVariables_ = lastRelinVariables_;
#endif
newISAM2->lastAffectedVariableCount = lastAffectedVariableCount;
newISAM2->lastAffectedFactorCount = lastAffectedFactorCount;
newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount;
newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount;
newISAM2->lastBacksubVariableCount = lastBacksubVariableCount;
newISAM2->lastNnzTop = lastNnzTop;
}
/**
* Add new factors, updating the solution and relinearizing as needed.
*

205
tests/testGaussianISAM2.cpp
View File

@ -28,6 +28,102 @@ using boost::shared_ptr;
const double tol = 1e-4;
ISAM2 createSlamlikeISAM2() {
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::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
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
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);
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);
}
// 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);
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);
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);
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);
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;
}
return isam;
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, AddVariables) {
@ -778,105 +874,30 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
/* ************************************************************************* */
TEST(ISAM2, clone) {
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
ISAM2 clone1;
// 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
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
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);
ISAM2 isam = createSlamlikeISAM2();
clone1 = isam;
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
ISAM2 clone2(isam);
isam.update(newfactors, init);
// Modify original isam
NonlinearFactorGraph factors;
factors.add(BetweenFactor<Pose2>(Symbol('x',0), Symbol('x',10),
isam.calculateEstimate<Pose2>(Symbol('x',0)).between(isam.calculateEstimate<Pose2>(Symbol('x',10))), sharedUnit(3)));
isam.update(factors);
CHECK(assert_equal(createSlamlikeISAM2(), clone2));
}
EXPECT(isam_check(fullgraph, fullinit, isam));
// This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced
// if the references in the iSAM2 copy point to the old instance which deleted at
// the end of the {...} section above.
ISAM2 temp = createSlamlikeISAM2();
// 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);
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);
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);
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);
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<ISAM2 > isam2
= boost::shared_ptr<ISAM2 >(new ISAM2());
isam.cloneTo(isam2);
CHECK(assert_equal(isam, *isam2));
CHECK(assert_equal(createSlamlikeISAM2(), clone1));
CHECK(assert_equal(clone1, temp));
}
/* ************************************************************************* */