InitializePose3.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/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;

struct GTSAM_EXPORT InitializePose3 {
  static GaussianFactorGraph buildLinearOrientationGraph(
      const NonlinearFactorGraph& g);

  static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);

  static Values computeOrientationsChordal(
      const NonlinearFactorGraph& pose3Graph);

  static Values computeOrientationsGradient(
      const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
      size_t maxIter = 10000, const bool setRefFrame = true);

  static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
                                  KeyRotMap& factorId2RotMap,
                                  const NonlinearFactorGraph& pose3Graph);

  static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
                              const double b);

  static NonlinearFactorGraph buildPose3graph(
      const NonlinearFactorGraph& graph);

  static Values computePoses(NonlinearFactorGraph& pose3graph,
                             Values& initialRot);

  static Values initializeOrientations(const NonlinearFactorGraph& graph);

  static Values initialize(const NonlinearFactorGraph& graph,
                           const Values& givenGuess, bool useGradient = false);

  static Values initialize(const NonlinearFactorGraph& graph);
};
}  // end of namespace gtsam