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

Finish timeLago

release/4.3a0
dellaert 2014-05-31 17:15:50 -04:00
parent 74fcc55d7a 76297ea2cc
commit bc69c0a94e
6 changed files with 152 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2857,6 +2857,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeLago.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>

60
gtsam/inference/VariableIndex-inl.h
View File

@ -23,43 +23,35 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices)
{
void VariableIndex::augment(const FG& factors,
boost::optional<const FastVector<size_t>&> newFactorIndices) {
gttic(VariableIndex_augment);
// Augment index for each factor
for(size_t i = 0; i < factors.size(); ++i)
{
if(factors[i])
{
for (size_t i = 0; i < factors.size(); ++i) {
if (factors[i]) {
const size_t globalI =
newFactorIndices ?
(*newFactorIndices)[i] :
nFactors_;
BOOST_FOREACH(const Key key, *factors[i])
{
newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
BOOST_FOREACH(const Key key, *factors[i]) {
index_[key].push_back(globalI);
++ nEntries_;
++nEntries_;
}
}
// Increment factor count even if factors are null, to keep indices consistent
if(newFactorIndices)
{
if((*newFactorIndices)[i] >= nFactors_)
if (newFactorIndices) {
if ((*newFactorIndices)[i] >= nFactors_)
nFactors_ = (*newFactorIndices)[i] + 1;
}
else
{
++ nFactors_;
} else {
++nFactors_;
}
}
}
/* ************************************************************************* */
template<typename ITERATOR, class FG>
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors)
{
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
// one greater than the highest-numbered factor referenced in a VariableIndex.
ITERATOR factorIndex = firstFactor;
size_t i = 0;
for( ; factorIndex != lastFactor; ++factorIndex, ++i) {
if(i >= factors.size())
throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if(factors[i]) {
for (; factorIndex != lastFactor; ++factorIndex, ++i) {
if (i >= factors.size())
throw std::invalid_argument(
"Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) {
BOOST_FOREACH(Key j, *factors[i]) {
Factors& factorEntries = internalAt(j);
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex);
if(entry == factorEntries.end())
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
Factors::iterator entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex);
if (entry == factorEntries.end())
throw std::invalid_argument(
"Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry);
-- nEntries_;
--nEntries_;
}
}
}
@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
/* ************************************************************************* */
template<typename ITERATOR>
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for(ITERATOR key = firstKey; key != lastKey; ++key) {
for (ITERATOR key = firstKey; key != lastKey; ++key) {
KeyMap::iterator entry = index_.find(*key);
if(!entry->second.empty())
throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
if (!entry->second.empty())
throw std::invalid_argument(
"Asking to remove variables from the variable index that are not unused");
index_.erase(entry);
}
}

65
gtsam/nonlinear/lago.cpp
View File

@ -21,9 +21,12 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <boost/math/special_functions.hpp>
using namespace std;
namespace gtsam {
namespace lago {
@ -32,7 +35,7 @@ static const Matrix I3 = eye(3);
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
noiseModel::Diagonal::Sigmas((Vector(1) << 0));
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
@ -76,7 +79,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
key2doubleMap thetaToRootMap;
// Orientation of the roo
thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree
BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
@ -84,14 +87,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap);
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
}
return thetaToRootMap;
}
/* ************************************************************************* */
void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
@ -112,10 +115,10 @@ void getSymbolicGraph(
// insert (directed) orientations in the map "deltaThetaMap"
bool inTree = false;
if (tree.at(key1) == key2) { // key2 -> key1
deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
inTree = true;
} else if (tree.at(key2) == key1) { // key1 -> key2
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta));
inTree = true;
}
// store factor slot, distinguishing spanning tree edges from chordsIds
@ -138,7 +141,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between)
throw std::invalid_argument(
throw invalid_argument(
"buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta());
@ -147,18 +150,17 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw std::invalid_argument(
"buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal noise model)!");
throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal noise model)!");
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
}
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree) {
const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
const PredecessorMap<Key>& tree) {
GaussianFactorGraph lagoGraph;
Vector deltaTheta;
@ -169,8 +171,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(
JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
}
// put regularized measurements in the chordsIds
BOOST_FOREACH(const size_t& factorId, chordsIds) {
@ -178,26 +179,24 @@ GaussianFactorGraph buildLinearOrientationGraph(
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1)
- orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = boost::math::round(k2pi_noise / (2 * M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
//if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
Vector deltaThetaRegularized = (Vector(1)
<< key1_DeltaTheta_key2 - 2 * k * M_PI);
lagoGraph.add(
JacobianFactor(key1, -I, key2, I, deltaThetaRegularized,
model_deltaTheta));
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
}
// prior on the anchor orientation
lagoGraph.add(
JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise));
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
return lagoGraph;
}
/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
gttic(lago_buildPose2graph);
NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
@ -250,6 +249,7 @@ static PredecessorMap<Key> findOdometricPath(
// Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) {
gttic(lago_computeOrientations);
// Find a minimum spanning tree
PredecessorMap<Key> tree;
@ -261,8 +261,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
// Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap;
std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
// temporary structure to correct wraparounds along loops
@ -293,6 +293,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
/* ************************************************************************* */
Values computePoses(const NonlinearFactorGraph& pose2graph,
VectorValues& orientationsLago) {
gttic(lago_computePoses);
// Linearized graph on full poses
GaussianFactorGraph linearPose2graph;
@ -319,8 +320,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
double dx = pose2Between->measured().x();
double dy = pose2Between->measured().y();
Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx
+ c1 * dy);
Vector globalDeltaCart = //
(Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy);
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
Matrix J1 = -I3;
J1(0, 2) = s1 * dx + c1 * dy;
@ -330,18 +331,15 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
boost::dynamic_pointer_cast<noiseModel::Diagonal>(
pose2Between->get_noiseModel());
linearPose2graph.add(
JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
} else {
throw std::invalid_argument(
throw invalid_argument(
"computeLagoPoses: cannot manage non between factor here!");
}
}
// add prior
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(
(Vector(3) << 1e-2, 1e-2, 1e-4));
linearPose2graph.add(
JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel));
linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
priorPose2Noise);
// optimize
VectorValues posesLago = linearPose2graph.optimize();
@ -362,6 +360,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
gttic(lago_initialize);
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph

2
gtsam/nonlinear/tests/CMakeLists.txt
View File

@ -1 +1 @@
gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam")

82
gtsam/nonlinear/tests/timeLago.cpp Normal file
View File

@ -0,0 +1,82 @@
/* ----------------------------------------------------------------------------
* 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 timeVirtual.cpp
* @brief Time the overhead of using virtual destructors and methods
* @author Richard Roberts
* @date Dec 3, 2010
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/lago.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/base/timing.h>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char *argv[]) {
size_t trials = 1;
// read graph
Values::shared_ptr solution;
NonlinearFactorGraph::shared_ptr g;
string inputFile = findExampleDataFile("w10000");
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(g, solution) = load2D(inputFile, model);
// add noise to create initial estimate
Values initial;
Sampler sampler(42u);
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// LAGO
for (size_t i = 0; i < trials; i++) {
{
gttic_(lago);
gttic_(init);
Values lagoInitial = lago::initialize(*g);
gttoc_(init);
gttic_(refine);
GaussNewtonOptimizer optimizer(*g, lagoInitial);
Values result = optimizer.optimize();
gttoc_(refine);
}
{
gttic_(optimize);
GaussNewtonOptimizer optimizer(*g, initial);
Values result = optimizer.optimize();
}
tictoc_finishedIteration_();
}
tictoc_print_();
return 0;
}

2
gtsam/slam/dataset.cpp
View File

@ -100,7 +100,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str());
if (!is)
throw std::invalid_argument("load2D: can not find the file!");
throw std::invalid_argument("load2D: can not find file " + filename);
Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);