Merge branch 'hybrid/error' into hybrid/tests

CMakeLists.txt

@@ -101,8 +101,6 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Copy matlab.h to the correct folder.
     configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
                ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
-    # Add the include directories so that matlab.h can be found
-    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
 
     add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")

INSTALL.md

@@ -50,7 +50,7 @@ will run up to 10x faster in Release mode! See the end of this document for
 additional debugging tips.
 
 3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your
-build directory.
+build directory after setting the `GTSAM_BUILD_DOCS` and `GTSAM_BUILD_[HTML|LATEX]` cmake flags.
 
 4. The instructions below install the library to the default system install path and
 build all components. From a terminal, starting in the root library folder,

cmake/Config.cmake.in

@@ -21,6 +21,10 @@ else()
 find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 endif()
 
+if(@GTSAM_USE_SYSTEM_EIGEN@)
+find_dependency(Eigen3 REQUIRED)
+endif()
+
 # Load exports
 include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)
 

cmake/FindEigen3.cmake

@@ -1,81 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
-#
-# Once done this will define
-#
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
-
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
-
-if(NOT Eigen3_FIND_VERSION)
-  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
-  endif(NOT Eigen3_FIND_VERSION_MAJOR)
-  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
-  endif(NOT Eigen3_FIND_VERSION_MINOR)
-  if(NOT Eigen3_FIND_VERSION_PATCH)
-    set(Eigen3_FIND_VERSION_PATCH 0)
-  endif(NOT Eigen3_FIND_VERSION_PATCH)
-
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
-endif(NOT Eigen3_FIND_VERSION)
-
-macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
-
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
-  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
-
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
-  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK FALSE)
-  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK TRUE)
-  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-
-  if(NOT EIGEN3_VERSION_OK)
-
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
-  endif(NOT EIGEN3_VERSION_OK)
-endmacro(_eigen3_check_version)
-
-if (EIGEN3_INCLUDE_DIR)
-
-  # in cache already
-  _eigen3_check_version()
-  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
-
-else (EIGEN3_INCLUDE_DIR)
-
-  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-      PATHS
-      ${CMAKE_INSTALL_PREFIX}/include
-      ${KDE4_INCLUDE_DIR}
-      PATH_SUFFIXES eigen3 eigen
-    )
-
-  if(EIGEN3_INCLUDE_DIR)
-    _eigen3_check_version()
-  endif(EIGEN3_INCLUDE_DIR)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
-
-  mark_as_advanced(EIGEN3_INCLUDE_DIR)
-
-endif(EIGEN3_INCLUDE_DIR)
-

cmake/HandleEigen.cmake

@@ -1,10 +1,6 @@
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-# Default: Use system's Eigen if found automatically:
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
-find_package(Eigen3 QUIET)
-set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
-option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
-unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
   # This option only makes sense if using the embedded copy of Eigen, it is
@@ -14,10 +10,14 @@ endif()
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-    find_package(Eigen3 REQUIRED) # need to find again as REQUIRED
+    # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
+    find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
 
-    # Use generic Eigen include paths e.g. <Eigen/Core>
+    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
+    # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
+    # versions.  So here I use the target itself to get the proper include
+    # directory (it is generated by cmake, thus has the correct path)
+    get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
 
     # check if MKL is also enabled - can have one or the other, but not both!
     # Note: Eigen >= v3.2.5 includes our patches
@@ -30,9 +30,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
         message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
     endif()
-
-    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 else()
     # Use bundled Eigen include path.
     # Clear any variables set by FindEigen3
@@ -45,7 +42,20 @@ else()
     set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
     # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen")
+
+    add_library(gtsam_eigen3 INTERFACE)
+
+    target_include_directories(gtsam_eigen3 INTERFACE
+      $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+      $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
+    )
+    add_library(Eigen3::Eigen ALIAS gtsam_eigen3)
+
+    install(TARGETS gtsam_eigen3 EXPORT GTSAM-exports PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+    list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3)
+    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}")
 endif()
 
 # Detect Eigen version:

doc/CMakeLists.txt

@@ -26,6 +26,7 @@ if (GTSAM_BUILD_DOCS)
         gtsam/basis 
         gtsam/discrete
         gtsam/geometry
+        gtsam/hybrid
         gtsam/inference
         gtsam/linear
         gtsam/navigation
@@ -33,7 +34,6 @@ if (GTSAM_BUILD_DOCS)
         gtsam/sam
         gtsam/sfm
         gtsam/slam
-        gtsam/smart
         gtsam/symbolic
         gtsam
     )
@@ -42,10 +42,12 @@ if (GTSAM_BUILD_DOCS)
     set(gtsam_unstable_doc_subdirs
         gtsam_unstable/base
         gtsam_unstable/discrete
+        gtsam_unstable/dynamics
+        gtsam_unstable/geometry
         gtsam_unstable/linear
-        gtsam_unstable/nonlinear 
+        gtsam_unstable/nonlinear
-        gtsam_unstable/slam 
+        gtsam_unstable/partition
-        gtsam_unstable/dynamics 
+        gtsam_unstable/slam
         gtsam_unstable
     )
     

doc/DoxygenLayout.xml

@@ -18,7 +18,6 @@
       <tab type="files" visible="yes" title="" intro=""/>
       <tab type="globals" visible="yes" title="" intro=""/>
     </tab>
-    <tab type="dirs" visible="yes" title="" intro=""/>
     <tab type="examples" visible="yes" title="" intro=""/>  
   </navindex>
 

gtsam/CMakeLists.txt

@@ -117,12 +117,10 @@ set_target_properties(gtsam PROPERTIES
     VERSION             ${gtsam_version}
     SOVERSION           ${gtsam_soversion})
 
-# Append Eigen include path, set in top-level CMakeLists.txt to either
+# Append Eigen include path to either
 # system-eigen, or GTSAM eigen path
-target_include_directories(gtsam PUBLIC
+target_link_libraries(gtsam PUBLIC Eigen3::Eigen)
-  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+
-  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
-)
 # MKL include dir:
 if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})

gtsam/base/ConcurrentMap.h

@@ -62,7 +62,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename KEY, typename VALUE>
 class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {

gtsam/base/DSFMap.h

@@ -28,7 +28,7 @@ namespace gtsam {
 /**
  * Disjoint set forest using an STL map data structure underneath
  * Uses rank compression and union by rank, iterator version
- * @addtogroup base
+ * @ingroup base
  */
 template <class KEY>
 class DSFMap {

gtsam/base/DSFVector.h

@@ -33,7 +33,7 @@ namespace gtsam {
  * A fast implementation of disjoint set forests that uses vector as underly data structure.
  * This is the absolute minimal DSF data structure, and only allows size_t keys
  * Uses rank compression but not union by rank :-(
- * @addtogroup base
+ * @ingroup base
  */
 class GTSAM_EXPORT DSFBase {
 
@@ -59,7 +59,7 @@ public:
 
 /**
  * DSFVector additionally keeps a vector of keys to support more expensive operations
- * @addtogroup base
+ * @ingroup base
  */
 class GTSAM_EXPORT DSFVector: public DSFBase {
 

gtsam/base/FastList.h

@@ -34,7 +34,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
-   * @addtogroup base
+   * @ingroup base
  */
 template<typename VALUE>
 class FastList: public std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> {

gtsam/base/FastMap.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename KEY, typename VALUE>
 class FastMap : public std::map<KEY, VALUE, std::less<KEY>,

gtsam/base/FastSet.h

@@ -43,7 +43,7 @@ namespace gtsam {
  * fast_pool_allocator instead of the default STL allocator.  This is just a
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several %.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename VALUE>
 class FastSet: public std::set<VALUE, std::less<VALUE>,

gtsam/base/FastVector.h

@@ -27,7 +27,7 @@ namespace gtsam {
 /**
  * FastVector is a type alias to a std::vector with a custom memory allocator.
  * The particular allocator depends on GTSAM's cmake configuration.
- * @addtogroup base
+ * @ingroup base
  */
 template <typename T>
 using FastVector =

gtsam/base/SymmetricBlockMatrix.h

@@ -47,7 +47,7 @@ namespace gtsam {
   * matrix view.  firstBlock() determines the block that appears to have index 0 for all operations
   * (except re-setting firstBlock()).
   *
-  * @addtogroup base */
+  * @ingroup base */
   class GTSAM_EXPORT SymmetricBlockMatrix
   {
   public:

gtsam/base/Testable.h

@@ -51,7 +51,7 @@ namespace gtsam {
    * tests and in generic algorithms.
    *
    * See macros for details on using this structure
-   * @addtogroup base
+   * @ingroup base
    * @tparam T is the objectype this constrains to be testable - assumes print() and equals()
    */
   template <class T>

gtsam/base/ThreadsafeException.h

@@ -14,7 +14,7 @@
  * @brief    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB
  * @author   Richard Roberts
  * @date     Aug 21, 2010
- * @addtogroup base
+ * @ingroup base
  */
 
 #pragma once

gtsam/base/VerticalBlockMatrix.h

@@ -38,7 +38,7 @@ namespace gtsam {
    * row for all operations.  To include all rows, rowEnd() should be set to the number of rows in
    * the matrix (i.e. one after the last true row index).
    *
-   * @addtogroup base */
+   * @ingroup base */
   class GTSAM_EXPORT VerticalBlockMatrix
   {
   public:

gtsam/base/make_shared.h

@@ -26,7 +26,7 @@
 #include <type_traits>
 
 namespace gtsam {
-  /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
+  /// An shorthand alias for accessing the \::type inside std::enable_if that can be used in a template directly
   template<bool B, class T = void>
   using enable_if_t = typename std::enable_if<B, T>::type;
 }

gtsam/base/timing.h

@@ -147,7 +147,7 @@ namespace gtsam {
       size_t id_;
       size_t t_;
       size_t tWall_;
-      double t2_ ; ///< cache the \sum t_i^2
+      double t2_ ; ///< cache the \f$ \sum t_i^2 \f$
       size_t tIt_;
       size_t tMax_;
       size_t tMin_;

gtsam/base/types.cpp

@@ -14,7 +14,7 @@
  * @brief    Functions for handling type information
  * @author   Varun Agrawal
  * @date     May 18, 2020
- * @addtogroup base
+ * @ingroup base
  */
 
 #include <gtsam/base/types.h>

gtsam/base/types.h

@@ -14,7 +14,7 @@
  * @brief    Typedefs for easier changing of types
  * @author   Richard Roberts
  * @date     Aug 21, 2010
- * @addtogroup base
+ * @ingroup base
  */
 
 #pragma once

gtsam/basis/Basis.h

@@ -78,6 +78,8 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
  * @tparam M Size of the identity matrix.
  * @param w The weights of the polynomial.
  * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]
+ *
+ * @ingroup basis
  */
 template <size_t M>
 Matrix kroneckerProductIdentity(const Weights& w) {
@@ -90,7 +92,10 @@ Matrix kroneckerProductIdentity(const Weights& w) {
   return result;
 }
 
-/// CRTP Base class for function bases
+/**
+ * CRTP Base class for function bases
+ *  @ingroup basis
+ */
 template <typename DERIVED>
 class Basis {
  public:

gtsam/basis/BasisFactors.h

@@ -32,6 +32,8 @@ namespace gtsam {
  *
  * Example, degree 8 Chebyshev polynomial measured at x=0.5:
  *  EvaluationFactor<Chebyshev2> factor(key, measured, model, 8, 0.5);
+ *
+ * @ingroup basis
  */
 template <class BASIS>
 class EvaluationFactor : public FunctorizedFactor<double, Vector> {
@@ -86,6 +88,8 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
  * @param M: Size of the evaluated state vector.
+ *
+ * @ingroup basis
  */
 template <class BASIS, int M>
 class VectorEvaluationFactor
@@ -149,6 +153,8 @@ class VectorEvaluationFactor
  *  VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
  *                                               N, i, t, a, b);
  *  where N is the degree and i is the component index.
+ *
+ * @ingroup basis
  */
 template <class BASIS, size_t P>
 class VectorComponentFactor

gtsam/basis/ParameterMatrix.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file ParamaterMatrix.h
+ * @file ParameterMatrix.h
  * @brief Define ParameterMatrix class which is used to store values at
  * interpolation points.
  * @author Varun Agrawal, Frank Dellaert

gtsam/discrete/AlgebraicDecisionTree.h

@@ -31,6 +31,8 @@ namespace gtsam {
    * Algebraic Decision Trees fix the range to double
    * Just has some nice constructors and some syntactic sugar
    * TODO: consider eliminating this class altogether?
+   *
+   * @ingroup discrete
    */
   template <typename L>
   class GTSAM_EXPORT AlgebraicDecisionTree : public DecisionTree<L, double> {
@@ -156,9 +158,9 @@ namespace gtsam {
     }
 
     /// print method customized to value type `double`.
-    void print(const std::string& s,
+    void print(const std::string& s = "",
-              const typename Base::LabelFormatter& labelFormatter =
+               const typename Base::LabelFormatter& labelFormatter =
-                  &DefaultFormatter) const {
+                   &DefaultFormatter) const {
       auto valueFormatter = [](const double& v) {
         return (boost::format("%4.8g") % v).str();
       };

gtsam/discrete/Assignment.h

@@ -31,6 +31,7 @@ namespace gtsam {
  * An assignment from labels to value index (size_t).
  * Assigns to each label a value. Implemented as a simple map.
  * A discrete factor takes an Assignment and returns a value.
+ * @ingroup discrete
  */
 template <class L>
 class Assignment : public std::map<L, size_t> {

gtsam/discrete/DecisionTree.h

@@ -38,6 +38,8 @@ namespace gtsam {
    * Decision Tree
    * L = label for variables
    * Y = function range (any algebra), e.g., bool, int, double
+   *
+   * @ingroup discrete
    */
   template<typename L, typename Y>
   class DecisionTree {

gtsam/discrete/DecisionTreeFactor.h

@@ -36,7 +36,9 @@ namespace gtsam {
   class DiscreteConditional;
 
   /**
-   * A discrete probabilistic factor
+   * A discrete probabilistic factor.
+   *
+   * @ingroup discrete
    */
   class GTSAM_EXPORT DecisionTreeFactor : public DiscreteFactor,
                                           public AlgebraicDecisionTree<Key> {

gtsam/discrete/DiscreteBayesNet.h

@@ -33,7 +33,7 @@ namespace gtsam {
 
 /** 
  * A Bayes net made from discrete conditional distributions. 
- * @addtogroup discrete
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
  public:

gtsam/discrete/DiscreteBayesTree.h

@@ -62,7 +62,10 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
 };
 
 /* ************************************************************************* */
-/** A Bayes tree representing a Discrete density */
+/**
+ * @brief A Bayes tree representing a Discrete density.
+ * @ingroup discrete
+ */
 class GTSAM_EXPORT DiscreteBayesTree
     : public BayesTree<DiscreteBayesTreeClique> {
  private:

gtsam/discrete/DiscreteConditional.h

@@ -32,6 +32,8 @@ namespace gtsam {
 /**
  * Discrete Conditional Density
  * Derives from DecisionTreeFactor
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteConditional
     : public DecisionTreeFactor,

gtsam/discrete/DiscreteDistribution.h

@@ -27,6 +27,8 @@ namespace gtsam {
 /**
  * A prior probability on a set of discrete variables.
  * Derives from DiscreteConditional
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
  public:

gtsam/discrete/DiscreteEliminationTree.h

@@ -24,6 +24,10 @@
 
 namespace gtsam {
 
+  /**
+   * @brief Elimination tree for discrete factors.
+   * @ingroup discrete
+   */
   class GTSAM_EXPORT DiscreteEliminationTree :
     public EliminationTree<DiscreteBayesNet, DiscreteFactorGraph>
   {

gtsam/discrete/DiscreteFactor.h

@@ -31,6 +31,8 @@ class DiscreteConditional;
 /**
  * Base class for discrete probabilistic factors
  * The most general one is the derived DecisionTreeFactor
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteFactor: public Factor {
 

gtsam/discrete/DiscreteFactorGraph.h

@@ -40,7 +40,14 @@ class DiscreteEliminationTree;
 class DiscreteBayesTree;
 class DiscreteJunctionTree;
 
-/** Main elimination function for DiscreteFactorGraph */
+/**
+ * @brief Main elimination function for DiscreteFactorGraph.
+ * 
+ * @param factors 
+ * @param keys 
+ * @return GTSAM_EXPORT
+ * @ingroup discrete
+ */
 GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
 EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys);
 
@@ -64,6 +71,7 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
 /**
  * A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
  *   Factor == DiscreteFactor
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteFactorGraph
     : public FactorGraph<DiscreteFactor>,

gtsam/discrete/DiscreteJunctionTree.h

@@ -44,7 +44,8 @@ namespace gtsam {
    * The tree structure and elimination method are exactly analogous to the EliminationTree,
    * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
    *
-   * \addtogroup Multifrontal
+   * \ingroup Multifrontal
+   * @ingroup discrete
    * \nosubgrouping
    */
   class GTSAM_EXPORT DiscreteJunctionTree :

gtsam/discrete/DiscreteKey.h

@@ -31,6 +31,7 @@ namespace gtsam {
   /**
    * Key type for discrete variables.
    * Includes Key and cardinality.
+   * @ingroup discrete
    */
   using DiscreteKey = std::pair<Key,size_t>;
 

gtsam/discrete/DiscreteLookupDAG.h

@@ -32,6 +32,7 @@ class DiscreteBayesNet;
 
 /**
  * @brief DiscreteLookupTable table for max-product
+ * @ingroup discrete
  *
  * Inherits from discrete conditional for convenience, but is not normalized.
  * Is used in the max-product algorithm.

gtsam/discrete/DiscreteMarginals.h

@@ -28,6 +28,7 @@ namespace gtsam {
 
   /**
    * A class for computing marginals of variables in a DiscreteFactorGraph
+   * @ingroup discrete
    */
 class DiscreteMarginals {
 

gtsam/discrete/DiscreteValues.h

@@ -36,6 +36,7 @@ namespace gtsam {
  * Another good thing is we don't need to have the special DiscreteKey which
  * stores cardinality of a Discrete variable. It should be handled naturally in
  * the new class DiscreteValue, as the variable's type (domain)
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
  public:

gtsam/discrete/Signature.h

@@ -48,6 +48,8 @@ namespace gtsam {
    *   (E|T,L) = "F F F 1"
    *   X|E = "95/5 2/98"
    *   (D|E,B) = "9/1 2/8 3/7 1/9"
+   *
+   * @ingroup discrete
    */
   class GTSAM_EXPORT Signature {
 

gtsam/geometry/Cal3.h

@@ -16,7 +16,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -63,7 +63,7 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
 
 /**
  * @brief Common base class for all calibration models.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3 {

gtsam/geometry/Cal3Bundler.h

@@ -26,7 +26,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration used by Bundler
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {

gtsam/geometry/Cal3DS2.h

@@ -29,7 +29,7 @@ namespace gtsam {
  * @brief Calibration of a camera with radial distortion that also supports
  * Lie-group behaviors for optimization.
  * \sa Cal3DS2_Base
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {

gtsam/geometry/Cal3DS2_Base.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration of a camera with radial distortion
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Uses same distortionmodel as OpenCV, with

gtsam/geometry/Cal3Fisheye.h

@@ -30,7 +30,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration of a fisheye camera
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Uses same distortionmodel as OpenCV, with

gtsam/geometry/Cal3Unified.h

@@ -18,7 +18,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -30,7 +30,7 @@ namespace gtsam {
 /**
  * @brief Calibration of a omni-directional camera with mirror + lens radial
  * distortion
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi

gtsam/geometry/Cal3_S2.h

@@ -16,7 +16,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -28,7 +28,7 @@ namespace gtsam {
 
 /**
  * @brief The most common 5DOF 3D->2D calibration
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {

gtsam/geometry/Cal3_S2Stereo.h

@@ -24,7 +24,7 @@ namespace gtsam {
 
 /**
  * @brief The most common 5DOF 3D->2D calibration, stereo version
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
@@ -38,7 +38,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
   /// @name Standard Constructors
-  /// @
+  /// @{
 
   /// default calibration leaves coordinates unchanged
   Cal3_S2Stereo() = default;
@@ -55,6 +55,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   Cal3_S2Stereo(double fov, int w, int h, double b)
       : Cal3_S2(fov, w, h), b_(b) {}
 
+  /// @}
+
   /**
    * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
@@ -82,7 +84,6 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
    */
   Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
 
-  /// @}
   /// @name Testable
   /// @{
 

gtsam/geometry/CalibratedCamera.h

@@ -46,7 +46,7 @@ private:
 
 /**
  * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT PinholeBase {
@@ -241,7 +241,7 @@ private:
  * A Calibrated camera class [R|-R't], calibration K=I.
  * If calibration is known, it is more computationally efficient
  * to calibrate the measurements rather than try to predict in pixels.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT CalibratedCamera: public PinholeBase {

gtsam/geometry/Line3.h

@@ -38,7 +38,7 @@ GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
 
 /**
  * A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Line3 {

gtsam/geometry/OrientedPlane3.h

@@ -133,8 +133,6 @@ public:
   inline double distance() const {
     return d_;
   }
-
-  /// @}
 };
 
 template<> struct traits<OrientedPlane3> : public internal::Manifold<

gtsam/geometry/PinholeCamera.h

@@ -26,7 +26,7 @@ namespace gtsam {
 /**
  * A pinhole camera class that has a Pose3 and a Calibration.
  * Use PinholePose if you will not be optimizing for Calibration
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename Calibration>

gtsam/geometry/PinholePose.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename CALIBRATION>
@@ -236,7 +236,7 @@ public:
  * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
  * Instead of using this class, one might consider calibrating the measurements
  * and using CalibratedCamera, which would then be faster.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename CALIBRATION>

gtsam/geometry/Pose2.h

@@ -30,7 +30,7 @@ namespace gtsam {
 
 /**
  * A 2D pose (Point2,Rot2)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class Pose2: public LieGroup<Pose2, 3> {

gtsam/geometry/Pose3.cpp

@@ -27,7 +27,6 @@
 namespace gtsam {
 
 using std::vector;
-using Point3Pairs = vector<Point3Pair>;
 
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3)

gtsam/geometry/Pose3.h

@@ -31,7 +31,7 @@ class Pose2;
 
 /**
  * A 3D pose (R,t) : (Rot3,Point3)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
@@ -83,7 +83,7 @@ public:
    *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
    *  Note this allows for noise on the points but in that case the mapping will not be exact.
    */
-  static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
+  static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
 
   // Version of Pose3::Align that takes 2 matrices.
   static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);

gtsam/geometry/Rot2.h

@@ -30,7 +30,7 @@ namespace gtsam {
   /**
    * Rotation matrix
    * NOTE: the angle theta is in radians unless explicitly stated
-   * @addtogroup geometry
+   * @ingroup geometry
    * \nosubgrouping
    */
   class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
@@ -86,7 +86,7 @@ namespace gtsam {
     static Rot2 atan2(double y, double x);
 
     /**
-     * Random, generates random angle \in [-p,pi]
+     * Random, generates random angle \f$\in\f$ [-pi,pi]
      * Example:
      *   std::mt19937 engine(42);
      *   Unit3 unit = Unit3::Random(engine);

gtsam/geometry/Rot3.h

@@ -53,7 +53,7 @@ namespace gtsam {
  * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
  * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
  * if it is defined.
- * @addtogroup geometry
+ * @ingroup geometry
  */
 class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
  private:
@@ -129,7 +129,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
 
     /**
-     * Random, generates a random axis, then random angle \in [-p,pi]
+     * Random, generates a random axis, then random angle \f$\in\f$ [-pi,pi]
      * Example:
      *   std::mt19937 engine(42);
      *   Unit3 unit = Unit3::Random(engine);

gtsam/geometry/SO4.h

@@ -74,7 +74,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::non
 
 /**
  * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
- * -> S \in St(3,4).
+ * -> \f$ S \in St(3,4) \f$.
  */
 GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
 

gtsam/geometry/SOn.h

@@ -121,7 +121,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// currently only defined for SO3.
   static SO ClosestTo(const MatrixNN& M);
 
-  /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F),
+  /// Named constructor that finds chordal mean
+  /// \f$ mu = argmin_R \sum sqr(|R-R_i|_F) \f$,
   /// currently only defined for SO3.
   static SO ChordalMean(const std::vector<SO>& rotations);
 

gtsam/geometry/Similarity2.h

@@ -132,7 +132,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
    * using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+  static Similarity2 Align(const Pose2Pairs& abPosePairs);
 
   /// @}
   /// @name Lie Group

gtsam/geometry/Similarity3.h

@@ -120,7 +120,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /**
    *  Create Similarity3 by aligning at least three point pairs
    */
-  static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+  static Similarity3 Align(const Point3Pairs& abPointPairs);
 
   /**
    * Create the Similarity3 object that aligns at least two pose pairs.

gtsam/geometry/SphericalCamera.h

@@ -34,7 +34,7 @@ namespace gtsam {
  * Empty calibration. Only needed to play well with other cameras
  * (e.g., when templating functions wrt cameras), since other cameras
  * have constuctors in the form ‘camera(pose,calibration)’
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT EmptyCal {
@@ -64,7 +64,7 @@ class GTSAM_EXPORT EmptyCal {
 /**
  * A spherical camera class that has a Pose3 and measures bearing vectors.
  * The camera has an ‘Empty’ calibration and the only 6 dof are the pose
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT SphericalCamera {
@@ -82,7 +82,6 @@ class GTSAM_EXPORT SphericalCamera {
   EmptyCal::shared_ptr emptyCal_;
 
  public:
-  /// @}
   /// @name Standard Constructors
   /// @{
 

gtsam/geometry/StereoCamera.h

@@ -42,7 +42,7 @@ private:
 
 /**
  * A stereo camera class, parameterize by left camera pose and stereo calibration
- * @addtogroup geometry
+ * @ingroup geometry
  */
 class GTSAM_EXPORT StereoCamera {
 

gtsam/geometry/StereoPoint2.h

@@ -26,7 +26,7 @@ namespace gtsam {
 
 /**
  * A 2D stereo point, v will be same for rectified images
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT StereoPoint2 {

gtsam/geometry/geometry.i

@@ -1123,7 +1123,7 @@ class StereoCamera {
 #include <gtsam/geometry/triangulation.h>
 class TriangulationResult {
   enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
-  Status status;
+  gtsam::TriangulationResult::Status status;
   TriangulationResult(const gtsam::Point3& p);
   const gtsam::Point3& get() const;
   static gtsam::TriangulationResult Degenerate();
@@ -1142,7 +1142,7 @@ class TriangulationParameters {
   bool enableEPI;
   double landmarkDistanceThreshold;
   double dynamicOutlierRejectionThreshold;
-  SharedNoiseModel noiseModel;
+  gtsam::SharedNoiseModel noiseModel;
   TriangulationParameters(const double rankTolerance = 1.0,
                           const bool enableEPI = false,
                           double landmarkDistanceThreshold = -1,

gtsam/global_includes.h

@@ -13,7 +13,7 @@
  * @file     global_includes.h
  * @brief    Included from all GTSAM files
  * @author   Richard Roberts
- * @addtogroup base
+ * @ingroup base
  */
 
 #pragma once

gtsam/groups.dox

@@ -1,7 +1,7 @@
 /**
 
 \defgroup LinearSolving Solving of sparse linear systems with least-squares
-@{
+@{ @}
 
 \defgroup Multifrontal Solving by multifrontal variable elimination (QR and Cholesky)
 @{ @}
@@ -9,9 +9,48 @@
 \defgroup Sequential Solving by sequential variable elimination (QR and Cholesky)
 @{ @}
 
-@}
+\defgroup base Base
-
-\defgroup SLAM Useful SLAM components
 @{ @}
 
-*/
+\defgroup basis Basis
+@{ @}
+
+\defgroup discrete Discrete
+@{ @}
+
+\defgroup geometry Geometry
+@{
+  Geometric primitives.
+@}
+
+\defgroup hybrid Hybrid
+@{ @}
+
+\defgroup inference Inference
+@{ @}
+
+\defgroup linear Linear
+@{ @}
+
+\defgroup navigation Navigation
+@{ @}
+
+\defgroup nonlinear Nonlinear
+@{ @}
+
+\defgroup sam SAM
+@{ @}
+
+\defgroup sfm SFM
+@{ @}
+
+\defgroup slam SLAM
+@{ Useful SLAM components @}
+
+\defgroup symbolic Symbolic
+@{ @}
+
+\defgroup isam2 ISAM2
+@{ @}
+
+*/ 

gtsam/hybrid/GaussianMixture.cpp

@@ -207,4 +207,28 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
   conditionals_.root_ = pruned_conditionals.root_;
 }
 
+/* *******************************************************************************/
+AlgebraicDecisionTree<Key> GaussianMixture::error(
+    const VectorValues &continuousVals) const {
+  // functor to convert from GaussianConditional to double error value.
+  auto errorFunc =
+      [continuousVals](const GaussianConditional::shared_ptr &conditional) {
+        if (conditional) {
+          return conditional->error(continuousVals);
+        } else {
+          // return arbitrarily large error
+          return 1e50;
+        }
+      };
+  DecisionTree<Key, double> errorTree(conditionals_, errorFunc);
+  return errorTree;
+}
+
+/* *******************************************************************************/
+double GaussianMixture::error(const VectorValues &continuousVals,
+                              const DiscreteValues &discreteValues) const {
+  auto conditional = conditionals_(discreteValues);
+  return conditional->error(continuousVals);
+}
+
 }  // namespace gtsam

gtsam/hybrid/GaussianMixture.h

@@ -45,6 +45,7 @@ namespace gtsam {
  * where i indexes the components and k_i is a component-wise normalization
  * constant.
  *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT GaussianMixture
     : public HybridFactor,
@@ -143,6 +144,26 @@ class GTSAM_EXPORT GaussianMixture
   /// Getter for the underlying Conditionals DecisionTree
   const Conditionals &conditionals();
 
+  /**
+   * @brief Compute error of the GaussianMixture as a tree.
+   *
+   * @param continuousVals The continuous VectorValues.
+   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * as the factor but leaf values as the error.
+   */
+  AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
+
+  /**
+   * @brief Compute the error of this Gaussian Mixture given the continuous
+   * values and a discrete assignment.
+   *
+   * @param continuousVals The continuous values at which to compute the error.
+   * @param discreteValues The discrete assignment for a specific mode sequence.
+   * @return double
+   */
+  double error(const VectorValues &continuousVals,
+               const DiscreteValues &discreteValues) const;
+
   /**
    * @brief Prune the decision tree of Gaussian factors as per the discrete
    * `decisionTree`.

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -95,4 +95,24 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
   };
   return {factors_, wrap};
 }
+
+/* *******************************************************************************/
+AlgebraicDecisionTree<Key> GaussianMixtureFactor::error(
+    const VectorValues &continuousVals) const {
+  // functor to convert from sharedFactor to double error value.
+  auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) {
+    return factor->error(continuousVals);
+  };
+  DecisionTree<Key, double> errorTree(factors_, errorFunc);
+  return errorTree;
+}
+
+/* *******************************************************************************/
+double GaussianMixtureFactor::error(
+    const VectorValues &continuousVals,
+    const DiscreteValues &discreteValues) const {
+  auto factor = factors_(discreteValues);
+  return factor->error(continuousVals);
+}
+
 }  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.h

@@ -20,15 +20,19 @@
 
 #pragma once
 
+#include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/VectorValues.h>
 
 namespace gtsam {
 
 class GaussianFactorGraph;
 
+// Needed for wrapper.
 using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
 
 /**
@@ -40,6 +44,7 @@ using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
  * Represents the underlying Gaussian Mixture as a Decision Tree, where the set
  * of discrete variables indexes to the continuous gaussian distribution.
  *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
  public:
@@ -125,6 +130,26 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    */
   Sum add(const Sum &sum) const;
 
+  /**
+   * @brief Compute error of the GaussianMixtureFactor as a tree.
+   *
+   * @param continuousVals The continuous VectorValues.
+   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * as the factor but leaf values as the error.
+   */
+  AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
+
+  /**
+   * @brief Compute the error of this Gaussian Mixture given the continuous
+   * values and a discrete assignment.
+   *
+   * @param continuousVals The continuous values at which to compute the error.
+   * @param discreteValues The discrete assignment for a specific mode sequence.
+   * @return double
+   */
+  double error(const VectorValues &continuousVals,
+               const DiscreteValues &discreteValues) const;
+
   /// Add MixtureFactor to a Sum, syntactic sugar.
   friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) {
     sum = factor.add(sum);

gtsam/hybrid/HybridBayesNet.cpp

@@ -232,4 +232,45 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
   return gbn.optimize();
 }
 
+/* ************************************************************************* */
+double HybridBayesNet::error(const VectorValues &continuousValues,
+                             const DiscreteValues &discreteValues) const {
+  GaussianBayesNet gbn = this->choose(discreteValues);
+  return gbn.error(continuousValues);
+}
+
+/* ************************************************************************* */
+AlgebraicDecisionTree<Key> HybridBayesNet::error(
+    const VectorValues &continuousValues) const {
+  AlgebraicDecisionTree<Key> error_tree;
+
+  for (size_t idx = 0; idx < size(); idx++) {
+    AlgebraicDecisionTree<Key> conditional_error;
+    if (factors_.at(idx)->isHybrid()) {
+      // If factor is hybrid, select based on assignment.
+      GaussianMixture::shared_ptr gm = this->atMixture(idx);
+      conditional_error = gm->error(continuousValues);
+
+      if (idx == 0) {
+        error_tree = conditional_error;
+      } else {
+        error_tree = error_tree + conditional_error;
+      }
+
+    } else if (factors_.at(idx)->isContinuous()) {
+      // If continuous only, get the (double) error
+      // and add it to the error_tree
+      double error = this->atGaussian(idx)->error(continuousValues);
+      error_tree = error_tree.apply(
+          [error](double leaf_value) { return leaf_value + error; });
+
+    } else if (factors_.at(idx)->isDiscrete()) {
+      // If factor at `idx` is discrete-only, we skip.
+      continue;
+    }
+  }
+
+  return error_tree;
+}
+
 }  // namespace gtsam

gtsam/hybrid/HybridBayesNet.h

@@ -29,6 +29,8 @@ namespace gtsam {
 /**
  * A hybrid Bayes net is a collection of HybridConditionals, which can have
  * discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals.
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
  public:
@@ -122,6 +124,26 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves.
   HybridBayesNet prune(size_t maxNrLeaves);
 
+  /**
+   * @brief 0.5 * sum of squared Mahalanobis distances
+   * for a specific discrete assignment.
+   *
+   * @param continuousValues Continuous values at which to compute the error.
+   * @param discreteValues Discrete assignment for a specific mode sequence.
+   * @return double
+   */
+  double error(const VectorValues &continuousValues,
+               const DiscreteValues &discreteValues) const;
+
+  /**
+   * @brief Compute conditional error for each discrete assignment,
+   * and return as a tree.
+   *
+   * @param continuousValues Continuous values at which to compute the error.
+   * @return AlgebraicDecisionTree<Key>
+   */
+  AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
+
   /// @}
 
  private:

gtsam/hybrid/HybridBayesTree.h

@@ -34,8 +34,11 @@ class HybridConditional;
 class VectorValues;
 
 /* ************************************************************************* */
-/** A clique in a HybridBayesTree
+/**
+ * @brief A clique in a HybridBayesTree
  * which is a HybridConditional internally.
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridBayesTreeClique
     : public BayesTreeCliqueBase<HybridBayesTreeClique,

gtsam/hybrid/HybridConditional.h

@@ -54,6 +54,8 @@ namespace gtsam {
  *
  * A great reference to the type-erasure pattern is Eduaado Madrid's CppCon
  * talk (https://www.youtube.com/watch?v=s082Qmd_nHs).
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridConditional
     : public HybridFactor,

gtsam/hybrid/HybridDiscreteFactor.h

@@ -28,6 +28,8 @@ namespace gtsam {
  * A HybridDiscreteFactor is a thin container for DiscreteFactor, which allows
  * us to hide the implementation of DiscreteFactor and thus avoid diamond
  * inheritance.
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor {
  private:

gtsam/hybrid/HybridEliminationTree.h

@@ -25,6 +25,8 @@ namespace gtsam {
 
 /**
  * Elimination Tree type for Hybrid
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridEliminationTree
     : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {

gtsam/hybrid/HybridFactor.h

@@ -40,6 +40,8 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
  *  - HybridDiscreteFactor
  *  - GaussianMixtureFactor
  *  - GaussianMixture
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridFactor : public Factor {
  private:

gtsam/hybrid/HybridGaussianFactor.h

@@ -27,6 +27,8 @@ namespace gtsam {
  * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
  * a diamond inheritance i.e. an extra factor type that inherits from both
  * HybridFactor and GaussianFactor.
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
  private:

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -39,7 +39,14 @@ class DecisionTreeFactor;
 
 class JacobianFactor;
 
-/** Main elimination function for HybridGaussianFactorGraph */
+/**
+ * @brief Main elimination function for HybridGaussianFactorGraph.
+ * 
+ * @param factors The factor graph to eliminate.
+ * @param keys The elimination ordering.
+ * @return The conditional on the ordering keys and the remaining factors.
+ * @ingroup hybrid
+ */
 GTSAM_EXPORT
 std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
 EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
@@ -68,10 +75,12 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
 };
 
 /**
- * Gaussian Hybrid Factor Graph
+ * Hybrid Gaussian Factor Graph
  * -----------------------
  * This is the linearized version of a hybrid factor graph.
  * Everything inside needs to be hybrid factor or hybrid conditional.
+ *
+ * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridGaussianFactorGraph
     : public HybridFactorGraph,
@@ -120,13 +129,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   void add(JacobianFactor&& factor);
 
   /// Add a Jacobian factor as a shared ptr.
-  void add(boost::shared_ptr<JacobianFactor> factor);
+  void add(JacobianFactor::shared_ptr factor);
 
   /// Add a DecisionTreeFactor to the factor graph.
   void add(DecisionTreeFactor&& factor);
 
   /// Add a DecisionTreeFactor as a shared ptr.
-  void add(boost::shared_ptr<DecisionTreeFactor> factor);
+  void add(DecisionTreeFactor::shared_ptr factor);
 
   /**
    * Add a gaussian factor *pointer* to the internal gaussian factor graph

gtsam/hybrid/HybridGaussianISAM.cpp

@@ -38,6 +38,35 @@ HybridGaussianISAM::HybridGaussianISAM() {}
 HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
     : Base(bayesTree) {}
 
+/* ************************************************************************* */
+Ordering HybridGaussianISAM::GetOrdering(
+    HybridGaussianFactorGraph& factors,
+    const HybridGaussianFactorGraph& newFactors) {
+  // Get all the discrete keys from the factors
+  KeySet allDiscrete = factors.discreteKeys();
+
+  // Create KeyVector with continuous keys followed by discrete keys.
+  KeyVector newKeysDiscreteLast;
+  const KeySet newFactorKeys = newFactors.keys();
+  // Insert continuous keys first.
+  for (auto& k : newFactorKeys) {
+    if (!allDiscrete.exists(k)) {
+      newKeysDiscreteLast.push_back(k);
+    }
+  }
+  // Insert discrete keys at the end
+  std::copy(allDiscrete.begin(), allDiscrete.end(),
+            std::back_inserter(newKeysDiscreteLast));
+
+  const VariableIndex index(factors);
+
+  // Get an ordering where the new keys are eliminated last
+  Ordering ordering = Ordering::ColamdConstrainedLast(
+      index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
+      true);
+  return ordering;
+}
+
 /* ************************************************************************* */
 void HybridGaussianISAM::updateInternal(
     const HybridGaussianFactorGraph& newFactors,
@@ -54,7 +83,7 @@ void HybridGaussianISAM::updateInternal(
   }
 
   // Add the removed top and the new factors
-  FactorGraphType factors;
+  HybridGaussianFactorGraph factors;
   factors += bn;
   factors += newFactors;
 
@@ -63,32 +92,12 @@ void HybridGaussianISAM::updateInternal(
     factors += boost::make_shared<BayesTreeOrphanWrapper<Node>>(orphan);
   }
 
-  // Get all the discrete keys from the factors
-  KeySet allDiscrete = factors.discreteKeys();
-
-  // Create KeyVector with continuous keys followed by discrete keys.
-  KeyVector newKeysDiscreteLast;
-  // Insert continuous keys first.
-  for (auto& k : newFactorKeys) {
-    if (!allDiscrete.exists(k)) {
-      newKeysDiscreteLast.push_back(k);
-    }
-  }
-  // Insert discrete keys at the end
-  std::copy(allDiscrete.begin(), allDiscrete.end(),
-            std::back_inserter(newKeysDiscreteLast));
-
-  // Get an ordering where the new keys are eliminated last
   const VariableIndex index(factors);
-
   Ordering elimination_ordering;
   if (ordering) {
     elimination_ordering = *ordering;
   } else {
-    elimination_ordering = Ordering::ColamdConstrainedLast(
+    elimination_ordering = GetOrdering(factors, newFactors);
-        index,
-        KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
-        true);
   }
 
   // eliminate all factors (top, added, orphans) into a new Bayes tree

gtsam/hybrid/HybridGaussianISAM.h

@@ -26,6 +26,11 @@
 
 namespace gtsam {
 
+/**
+ * @brief 
+ *
+ * @ingroup hybrid
+ */
 class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
  public:
   typedef ISAM<HybridBayesTree> Base;
@@ -67,6 +72,17 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
               const boost::optional<Ordering>& ordering = boost::none,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
+
+  /**
+   * @brief Helper method to get an ordering given the existing factors and any
+   * new factors added.
+   *
+   * @param factors The existing factors in the BayesTree.
+   * @param newFactors New factors added during the update step.
+   * @return Ordering
+   */
+  static Ordering GetOrdering(HybridGaussianFactorGraph& factors,
+                              const HybridGaussianFactorGraph& newFactors);
 };
 
 /// traits

gtsam/hybrid/HybridJunctionTree.h

@@ -45,7 +45,8 @@ class HybridEliminationTree;
  * EliminationTree, except that in the JunctionTree, at each node multiple
  * variables are eliminated at a time.
  *
- * \addtogroup Multifrontal
+ * \ingroup Multifrontal
+ * \ingroup hybrid
  * \nosubgrouping
  */
 class GTSAM_EXPORT HybridJunctionTree

gtsam/hybrid/MixtureFactor.h

@@ -107,8 +107,12 @@ class MixtureFactor : public HybridFactor {
       std::copy(f->keys().begin(), f->keys().end(),
                 std::inserter(factor_keys_set, factor_keys_set.end()));
 
-      nonlinear_factors.push_back(
+      if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>(f)) {
-          boost::dynamic_pointer_cast<NonlinearFactor>(f));
+        nonlinear_factors.push_back(nf);
+      } else {
+        throw std::runtime_error(
+            "Factors passed into MixtureFactor need to be nonlinear!");
+      }
     }
     factors_ = Factors(discreteKeys, nonlinear_factors);
 
@@ -121,6 +125,22 @@ class MixtureFactor : public HybridFactor {
 
   ~MixtureFactor() = default;
 
+  /**
+   * @brief Compute error of the MixtureFactor as a tree.
+   *
+   * @param continuousVals The continuous values for which to compute the error.
+   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * as the factor but leaf values as the error.
+   */
+  AlgebraicDecisionTree<Key> error(const Values& continuousVals) const {
+    // functor to convert from sharedFactor to double error value.
+    auto errorFunc = [continuousVals](const sharedFactor& factor) {
+      return factor->error(continuousVals);
+    };
+    DecisionTree<Key, double> errorTree(factors_, errorFunc);
+    return errorTree;
+  }
+
   /**
    * @brief Compute error of factor given both continuous and discrete values.
    *
@@ -149,7 +169,7 @@ class MixtureFactor : public HybridFactor {
 
   /// print to stdout
   void print(
-      const std::string& s = "MixtureFactor",
+      const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << (s.empty() ? "" : s + " ");
     Base::print("", keyFormatter);

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -78,15 +78,51 @@ TEST(GaussianMixture, Equals) {
   GaussianMixture::Conditionals conditionals(
       {m1},
       vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
-  GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals);
+  GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals);
 
   // Let's check that this worked:
   DiscreteValues mode;
   mode[m1.first] = 1;
-  auto actual = mixtureFactor(mode);
+  auto actual = mixture(mode);
   EXPECT(actual == conditional1);
 }
 
+/* ************************************************************************* */
+/// Test error method of GaussianMixture.
+TEST(GaussianMixture, Error) {
+  Matrix22 S1 = Matrix22::Identity();
+  Matrix22 S2 = Matrix22::Identity() * 2;
+  Matrix22 R1 = Matrix22::Ones();
+  Matrix22 R2 = Matrix22::Ones();
+  Vector2 d1(1, 2), d2(2, 1);
+
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+
+  auto conditional0 = boost::make_shared<GaussianConditional>(X(1), d1, R1,
+                                                              X(2), S1, model),
+       conditional1 = boost::make_shared<GaussianConditional>(X(1), d2, R2,
+                                                              X(2), S2, model);
+
+  // Create decision tree
+  DiscreteKey m1(1, 2);
+  GaussianMixture::Conditionals conditionals(
+      {m1},
+      vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
+  GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals);
+
+  VectorValues values;
+  values.insert(X(1), Vector2::Ones());
+  values.insert(X(2), Vector2::Zero());
+  auto error_tree = mixture.error(values);
+
+  std::vector<DiscreteKey> discrete_keys = {m1};
+  std::vector<double> leaves = {0.5, 4.3252595};
+  AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
+
+  // regression
+  EXPECT(assert_equal(expected_error, error_tree, 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/hybrid/tests/testGaussianMixtureFactor.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    GaussianMixtureFactor.cpp
+ * @file    testGaussianMixtureFactor.cpp
  * @brief   Unit tests for GaussianMixtureFactor
  * @author  Varun Agrawal
  * @author  Fan Jiang
@@ -135,7 +135,7 @@ TEST(GaussianMixtureFactor, Printing) {
   EXPECT(assert_print_equal(expected, mixtureFactor));
 }
 
-TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
+TEST(GaussianMixtureFactor, GaussianMixture) {
   KeyVector keys;
   keys.push_back(X(0));
   keys.push_back(X(1));
@@ -151,6 +151,45 @@ TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
   EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
 }
 
+/* ************************************************************************* */
+// Test the error of the GaussianMixtureFactor
+TEST(GaussianMixtureFactor, Error) {
+  DiscreteKey m1(1, 2);
+
+  auto A01 = Matrix2::Identity();
+  auto A02 = Matrix2::Identity();
+
+  auto A11 = Matrix2::Identity();
+  auto A12 = Matrix2::Identity() * 2;
+
+  auto b = Vector2::Zero();
+
+  auto f0 = boost::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
+  auto f1 = boost::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
+  std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
+
+  GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
+
+  VectorValues continuousVals;
+  continuousVals.insert(X(1), Vector2(0, 0));
+  continuousVals.insert(X(2), Vector2(1, 1));
+
+  // error should return a tree of errors, with nodes for each discrete value.
+  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
+
+  std::vector<DiscreteKey> discrete_keys = {m1};
+  std::vector<double> errors = {1, 4};
+  AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
+
+  EXPECT(assert_equal(expected_error, error_tree));
+
+  // Test for single leaf given discrete assignment P(X|M,Z).
+  DiscreteValues discreteVals;
+  discreteVals[m1.first] = 1;
+  EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals),
+                       1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -183,6 +183,61 @@ TEST(HybridBayesNet, OptimizeMultifrontal) {
   EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5));
 }
 
+/* ****************************************************************************/
+// Test bayes net error
+TEST(HybridBayesNet, Error) {
+  Switching s(3);
+
+  Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
+  HybridBayesNet::shared_ptr hybridBayesNet =
+      s.linearizedFactorGraph.eliminateSequential(hybridOrdering);
+
+  HybridValues delta = hybridBayesNet->optimize();
+  auto error_tree = hybridBayesNet->error(delta.continuous());
+
+  std::vector<DiscreteKey> discrete_keys = {{M(0), 2}, {M(1), 2}};
+  std::vector<double> leaves = {0.0097568009, 3.3973404e-31, 0.029126214,
+                                0.0097568009};
+  AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
+
+  // regression
+  EXPECT(assert_equal(expected_error, error_tree, 1e-9));
+
+  // Error on pruned bayes net
+  auto prunedBayesNet = hybridBayesNet->prune(2);
+  auto pruned_error_tree = prunedBayesNet.error(delta.continuous());
+
+  std::vector<double> pruned_leaves = {2e50, 3.3973404e-31, 2e50, 0.0097568009};
+  AlgebraicDecisionTree<Key> expected_pruned_error(discrete_keys,
+                                                   pruned_leaves);
+
+  // regression
+  EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9));
+
+  // Verify error computation and check for specific error value
+  DiscreteValues discrete_values;
+  discrete_values[M(0)] = 1;
+  discrete_values[M(1)] = 1;
+
+  double total_error = 0;
+  for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {
+    if (hybridBayesNet->at(idx)->isHybrid()) {
+      double error = hybridBayesNet->atMixture(idx)->error(delta.continuous(),
+                                                           discrete_values);
+      total_error += error;
+    } else if (hybridBayesNet->at(idx)->isContinuous()) {
+      double error = hybridBayesNet->atGaussian(idx)->error(delta.continuous());
+      total_error += error;
+    }
+  }
+
+  EXPECT_DOUBLES_EQUAL(
+      total_error, hybridBayesNet->error(delta.continuous(), discrete_values),
+      1e-9);
+  EXPECT_DOUBLES_EQUAL(total_error, error_tree(discrete_values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(total_error, pruned_error_tree(discrete_values), 1e-9);
+}
+
 /* ****************************************************************************/
 // Test bayes net pruning
 TEST(HybridBayesNet, Prune) {

gtsam/hybrid/tests/testMixtureFactor.cpp

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testMixtureFactor.cpp
+ * @brief   Unit tests for MixtureFactor
+ * @author  Varun Agrawal
+ * @date    October 2022
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/MixtureFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using noiseModel::Isotropic;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+/* ************************************************************************* */
+// Check iterators of empty mixture.
+TEST(MixtureFactor, Constructor) {
+  MixtureFactor factor;
+  MixtureFactor::const_iterator const_it = factor.begin();
+  CHECK(const_it == factor.end());
+  MixtureFactor::iterator it = factor.begin();
+  CHECK(it == factor.end());
+}
+
+
+TEST(MixtureFactor, Printing) {
+  DiscreteKey m1(1, 2);
+  double between0 = 0.0;
+  double between1 = 1.0;
+
+  Vector1 sigmas = Vector1(1.0);
+  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
+
+  auto f0 =
+      boost::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
+  auto f1 =
+      boost::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
+  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
+
+  MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
+
+  std::string expected =
+      R"(Hybrid [x1 x2; 1]
+MixtureFactor
+ Choice(1) 
+ 0 Leaf Nonlinear factor on 2 keys
+ 1 Leaf Nonlinear factor on 2 keys
+)";
+  EXPECT(assert_print_equal(expected, mixtureFactor));
+}
+
+/* ************************************************************************* */
+// Test the error of the MixtureFactor
+TEST(MixtureFactor, Error) {
+  DiscreteKey m1(1, 2);
+
+  double between0 = 0.0;
+  double between1 = 1.0;
+
+  Vector1 sigmas = Vector1(1.0);
+  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
+
+  auto f0 =
+      boost::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
+  auto f1 =
+      boost::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
+  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
+
+  MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
+
+  Values continuousVals;
+  continuousVals.insert<double>(X(1), 0);
+  continuousVals.insert<double>(X(2), 1);
+
+  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
+
+  std::vector<DiscreteKey> discrete_keys = {m1};
+  std::vector<double> errors = {0.5, 0};
+  AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
+
+  EXPECT(assert_equal(expected_error, error_tree));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/inference/BayesNet.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * A BayesNet is a tree of conditionals, stored in elimination order.
- * @addtogroup inference
+ * @ingroup inference
  */
 template <class CONDITIONAL>
 class BayesNet : public FactorGraph<CONDITIONAL> {

gtsam/inference/BayesTree-inst.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    BayesTree-inl.h
+ * @file    BayesTree-inst.h
  * @brief   Bayes Tree is a tree of cliques of a Bayes Chain
  * @author  Frank Dellaert
  * @author  Michael Kaess

gtsam/inference/BayesTree.h

@@ -59,7 +59,7 @@ namespace gtsam {
    * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
    * as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
    *
-   * \addtogroup Multifrontal
+   * \ingroup Multifrontal
    * \nosubgrouping
    */
   template<class CLIQUE>

gtsam/inference/ClusterTree-inst.h

@@ -1,5 +1,5 @@
 /**
- * @file EliminatableClusterTree-inst.h
+ * @file ClusterTree-inst.h
  * @date Oct 8, 2013
  * @author Kai Ni
  * @author Richard Roberts

gtsam/inference/ClusterTree.h

@@ -1,5 +1,5 @@
 /**
- * @file EliminatableClusterTree.h
+ * @file ClusterTree.h
  * @date Oct 8, 2013
  * @author Kai Ni
  * @author Richard Roberts
@@ -139,7 +139,6 @@ class ClusterTree {
              const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
   /// @}
-
   /// @name Advanced Interface
   /// @{
 
@@ -169,6 +168,7 @@ class ClusterTree {
 
  protected:
   /// @name Details
+  /// @{
 
   /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
   /// are copied, factors are not cloned.
@@ -236,6 +236,7 @@ class EliminatableClusterTree : public ClusterTree<GRAPH> {
 
  protected:
   /// @name Details
+  /// @{
 
   /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
   /// are copied, factors are not cloned.

gtsam/inference/DotWriter.h

@@ -30,7 +30,7 @@ namespace gtsam {
 
 /**
  * @brief DotWriter is a helper class for writing graphviz .dot files.
- * @addtogroup inference
+ * @ingroup inference
  */
 struct GTSAM_EXPORT DotWriter {
   double figureWidthInches;   ///< The figure width on paper in inches

gtsam/inference/EliminationTree-inst.h

@@ -10,7 +10,7 @@
 * -------------------------------------------------------------------------- */
 
 /**
-* @file    EliminationTree-inl.h
+* @file    EliminationTree-inst.h
 * @author  Frank Dellaert
 * @author  Richard Roberts
 * @date    Oct 13, 2010