100 lines
3.3 KiB
C++
100 lines
3.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file pose2SLAM.cpp
|
|
* @brief: Odometry measurements in 2D plane
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include <gtsam/linear/SimpleSPCGSolver.h>
|
|
#include <gtsam/slam/pose2SLAM.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
|
|
// Use pose2SLAM namespace for specific SLAM instance
|
|
|
|
namespace pose2SLAM {
|
|
|
|
/* ************************************************************************* */
|
|
Values Values::Circle(size_t n, double R) {
|
|
Values x;
|
|
double theta = 0, dtheta = 2 * M_PI / n;
|
|
for (size_t i = 0; i < n; i++, theta += dtheta)
|
|
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
|
|
return x;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Values::xs() const {
|
|
size_t j=0;
|
|
Vector result(size());
|
|
ConstFiltered<Pose2> poses = filter<Pose2>();
|
|
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
|
|
result(j++) = keyValue.value.x();
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Values::ys() const {
|
|
size_t j=0;
|
|
Vector result(size());
|
|
ConstFiltered<Pose2> poses = filter<Pose2>();
|
|
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
|
|
result(j++) = keyValue.value.y();
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Values::thetas() const {
|
|
size_t j=0;
|
|
Vector result(size());
|
|
ConstFiltered<Pose2> poses = filter<Pose2>();
|
|
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
|
|
result(j++) = keyValue.value.theta ();
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
|
|
sharedFactor factor(new Prior(i, p, model));
|
|
push_back(factor);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Graph::addPoseConstraint(Key i, const Pose2& p) {
|
|
sharedFactor factor(new HardConstraint(i, p));
|
|
push_back(factor);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Graph::addOdometry(Key i1, Key i2, const Pose2& z,
|
|
const SharedNoiseModel& model) {
|
|
sharedFactor factor(new Odometry(i1, i2, z, model));
|
|
push_back(factor);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Values Graph::optimize(const Values& initialEstimate) const {
|
|
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
|
|
}
|
|
|
|
Values Graph::optimizeSPCG(const Values& initialEstimate) const {
|
|
LevenbergMarquardtParams params;
|
|
params.linearSolverType = SuccessiveLinearizationParams::CG;
|
|
params.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
|
|
return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
|
|
}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // pose2SLAM
|