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

addConstraint

release/4.3a0
Frank Dellaert 2012-05-22 04:30:30 +00:00
parent 6be07e33bf
commit 9caf04dccd
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/pose2SLAM.cpp
View File

@ -16,6 +16,7 @@
**/ **/
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
@ -51,5 +52,10 @@ namespace pose2SLAM {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate) const {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
/* ************************************************************************* */
} // pose2SLAM } // pose2SLAM

13
gtsam/slam/pose2SLAM.h
View File

@ -17,14 +17,13 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/geometry/Pose2.h>
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM { namespace pose2SLAM {
@ -93,10 +92,14 @@ namespace pose2SLAM {
void addOdometry(Index i, Index j, const Pose2& z, void addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model); const SharedNoiseModel& model);
/// Optimize /// AddConstraint adds a soft constraint between factor between keys i and j
Values optimize(const Values& initialEstimate) const { void addConstraint(Index i, Index j, const Pose2& z,
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); const SharedNoiseModel& model) {
addOdometry(i,j,z,model); // same for now
} }
/// Optimize
Values optimize(const Values& initialEstimate) const;
}; };
} // pose2SLAM } // pose2SLAM