lago.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

namespace gtsam {
namespace lago {

typedef std::map<Key, double> key2doubleMap;
typedef std::map<Key, Key> PredecessorMap;

GTSAM_EXPORT key2doubleMap computeThetasToRoot(
    const key2doubleMap& deltaThetaMap, const PredecessorMap& tree);

GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
    key2doubleMap& deltaThetaMap,
    /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g);

GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
    const std::vector<size_t>& spanningTreeIds,
    const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
    const key2doubleMap& orientationsToRoot, const PredecessorMap& tree);

GTSAM_EXPORT PredecessorMap findMinimumSpanningTree(
    const NonlinearFactorGraph& pose2Graph);

GTSAM_EXPORT VectorValues initializeOrientations(
    const NonlinearFactorGraph& graph, bool useOdometricPath = true);

GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
    bool useOdometricPath = true);

GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
    const Values& initialGuess);

} // end of namespace lago
} // end of namespace gtsam