Enable GNC Optimizer

gtsam/CMakeLists.txt

@@ -59,7 +59,6 @@ endif()
 # if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following:
 if(NOT GTSAM_USE_BOOST_FEATURES)
     list (APPEND excluded_sources
-        "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h"
         "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h"
         "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h"
     )

gtsam/nonlinear/GncOptimizer.h

@@ -28,16 +28,15 @@
 
 #include <gtsam/nonlinear/GncParams.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <boost/math/distributions/chi_squared.hpp>
+#include <gtsam/nonlinear/internal/chiSquaredInverse.h>
 
 namespace gtsam {
 /*
  * Quantile of chi-squared distribution with given degrees of freedom at probability alpha.
  * Equivalent to chi2inv in Matlab.
  */
 static double Chi2inv(const double alpha, const size_t dofs) {
-  boost::math::chi_squared_distribution<double> chi2(dofs);
-  return boost::math::quantile(chi2, alpha);
+  return internal::chi_squared_quantile(dofs, alpha);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/GncParams.h

@@ -76,9 +76,9 @@ class GncParams {
   /// Use IndexVector for inliers and outliers since it is fast
   using IndexVector = FastVector<uint64_t>;
   ///< Slots in the factor graph corresponding to measurements that we know are inliers
-  IndexVector knownInliers = IndexVector();
+  IndexVector knownInliers;
   ///< Slots in the factor graph corresponding to measurements that we know are outliers
-  IndexVector knownOutliers = IndexVector();
+  IndexVector knownOutliers;
 
   /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
   void setLossType(const GncLossType type) {

gtsam/nonlinear/internal/Gamma.h

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    Gamma.h
+ * @brief   Gamma and Gamma Inverse functions
+ *
+ * A lot of this code has been picked up from
+ * https://www.boost.org/doc/libs/1_83_0/boost/math/special_functions/detail/igamma_inverse.hpp
+ *
+ * @author  Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/internal/Utils.h>
+
+#include <boost/math/special_functions/gamma.hpp>
+
+namespace gtsam {
+
+namespace internal {
+
+/**
+ * @brief Functional to compute the gamma inverse.
+ * Mainly used with Halley iteration.
+ *
+ * @tparam T
+ */
+template <class T>
+struct gamma_p_inverse_func {
+  gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv) {
+    /*
+    If p is too near 1 then P(x) - p suffers from cancellation
+    errors causing our root-finding algorithms to "thrash", better
+    to invert in this case and calculate Q(x) - (1-p) instead.
+
+    Of course if p is *very* close to 1, then the answer we get will
+    be inaccurate anyway (because there's not enough information in p)
+    but at least we will converge on the (inaccurate) answer quickly.
+    */
+    if (p > T(0.9)) {
+      p = 1 - p;
+      invert = !invert;
+    }
+  }
+
+  std::tuple<T, T, T> operator()(const T& x) const {
+    // Calculate P(x) - p and the first two derivates, or if the invert
+    // flag is set, then Q(x) - q and it's derivatives.
+    T f, f1;
+    T ft;
+    boost::math::policies::policy<> pol;
+    f = static_cast<T>(boost::math::detail::gamma_incomplete_imp(
+        a, x, true, invert, pol, &ft));
+    f1 = ft;
+    T f2;
+    T div = (a - x - 1) / x;
+    f2 = f1;
+
+    if (fabs(div) > 1) {
+      if (internal::LIM<T>::max() / fabs(div) < f2) {
+        // overflow:
+        f2 = -internal::LIM<T>::max() / 2;
+      } else {
+        f2 *= div;
+      }
+    } else {
+      f2 *= div;
+    }
+
+    if (invert) {
+      f1 = -f1;
+      f2 = -f2;
+    }
+
+    return std::make_tuple(static_cast<T>(f - p), f1, f2);
+  }
+
+ private:
+  T a, p;
+  bool invert;
+};
+
+}  // namespace internal
+}  // namespace gtsam

gtsam/nonlinear/internal/Utils.h

@@ -0,0 +1,49 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    Utils.h
+ * @brief   Utilities for the Chi Squared inverse and related operations.
+ * @author  Varun Agrawal
+ */
+
+#pragma once
+
+namespace gtsam {
+namespace internal {
+
+/// Template type for numeric limits
+template <class T>
+using LIM = std::numeric_limits<T>;
+
+template <typename T>
+using return_t =
+    typename std::conditional<std::is_integral<T>::value, double, T>::type;
+
+/// Get common type amongst all arguments
+template <typename... T>
+using common_t = typename std::common_type<T...>::type;
+
+/// Helper template for finding common return type
+template <typename... T>
+using common_return_t = return_t<common_t<T...>>;
+
+/// Check if integer is odd
+constexpr bool is_odd(const long long int x) noexcept { return (x & 1U) != 0; }
+
+/// Templated check for NaN
+template <typename T>
+constexpr bool is_nan(const T x) noexcept {
+  return x != x;
+}
+
+}  // namespace internal
+}  // namespace gtsam