85 lines
2.7 KiB
C++
85 lines
2.7 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 InitializePose3.h
|
|
* @brief Initialize Pose3 in a factor graph
|
|
*
|
|
* @author Luca Carlone
|
|
* @author Frank Dellaert
|
|
* @date August, 2014
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/geometry/Rot3.h>
|
|
#include <gtsam/inference/graph.h>
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
|
|
namespace gtsam {
|
|
|
|
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
|
|
typedef std::map<Key, Rot3> KeyRotMap;
|
|
|
|
namespace initialize_pose3 {
|
|
|
|
GTSAM_EXPORT GaussianFactorGraph
|
|
buildLinearOrientationGraph(const NonlinearFactorGraph& g);
|
|
|
|
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
|
|
|
|
/**
|
|
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
|
*/
|
|
GTSAM_EXPORT Values
|
|
computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
|
|
|
|
/**
|
|
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
|
*/
|
|
GTSAM_EXPORT Values computeOrientationsGradient(
|
|
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
|
|
size_t maxIter = 10000, const bool setRefFrame = true);
|
|
|
|
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
|
|
KeyRotMap& factorId2RotMap,
|
|
const NonlinearFactorGraph& pose3Graph);
|
|
|
|
GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2,
|
|
const double a, const double b);
|
|
|
|
/**
|
|
* Select the subgraph of betweenFactors and transforms priors into between wrt
|
|
* a fictitious node
|
|
*/
|
|
GTSAM_EXPORT NonlinearFactorGraph
|
|
buildPose3graph(const NonlinearFactorGraph& graph);
|
|
|
|
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,
|
|
Values& initialRot);
|
|
|
|
/**
|
|
* "extract" the Pose3 subgraph of the original graph, get orientations from
|
|
* relative orientation measurements (using either gradient or chordal method),
|
|
* and finish up with 1 GN iteration on full poses.
|
|
*/
|
|
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
|
|
const Values& givenGuess,
|
|
bool useGradient = false);
|
|
|
|
/// Calls initialize above using Chordal method.
|
|
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
|
|
|
|
} // namespace initialize_pose3
|
|
} // end of namespace gtsam
|