ShonanFactor.h

Go to the documentation of this file.

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

 * GTSAM Copyright 2010-2019, 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/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

#include <type_traits>

namespace gtsam {

template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
  Matrix M_;                    
  size_t p_, pp_;               
  std::shared_ptr<Matrix> G_; 

  // Select Rot2 or Rot3 interface based template parameter d
  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;

public:

  // Provide access to the Matrix& version of evaluateError:
  using NoiseModelFactor2<SOn, SOn>::evaluateError;


  ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
               const SharedNoiseModel &model = nullptr,
               const std::shared_ptr<Matrix> &G = nullptr);


  void
  print(const std::string &s,
        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;

  bool equals(const NonlinearFactor &expected,
              double tol = 1e-9) const override;


  Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override;

private:
  void fillJacobians(const Matrix &M1, const Matrix &M2,
                     OptionalMatrixType H1,
                     OptionalMatrixType H2) const;
};

// Explicit instantiation for d=2 and d=3 in .cpp file:
using ShonanFactor2 = ShonanFactor<2>;
using ShonanFactor3 = ShonanFactor<3>;

} // namespace gtsam