Merge branch 'develop' into feature/nonlinear-hybrid

.github/scripts/python.sh

@@ -43,46 +43,68 @@ if [ -z ${PYTHON_VERSION+x} ]; then
     exit 127
 fi
 
-PYTHON="python${PYTHON_VERSION}"
+export PYTHON="python${PYTHON_VERSION}"
 
-if [[ $(uname) == "Darwin" ]]; then
+function install_dependencies()
+{
+  if [[ $(uname) == "Darwin" ]]; then
     brew install wget
-else
+  else
     # Install a system package required by our library
     sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
-fi
+  fi
 
-PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
+  export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
 
-[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+  [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+
+  $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+}
+
+function build()
+{
+  mkdir $GITHUB_WORKSPACE/build
+  cd $GITHUB_WORKSPACE/build
+
+  BUILD_PYBIND="ON"
+
+  cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+      -DGTSAM_BUILD_TESTS=OFF \
+      -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_USE_QUATERNIONS=OFF \
+      -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+      -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+      -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
+      -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+      -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+      -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
 
 
-BUILD_PYBIND="ON"
+  # Set to 2 cores so that Actions does not error out during resource provisioning.
+  make -j2 install
 
-sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+  cd $GITHUB_WORKSPACE/build/python
+  $PYTHON -m pip install --user .
+}
 
-mkdir $GITHUB_WORKSPACE/build
+function test()
-cd $GITHUB_WORKSPACE/build
+{
+  cd $GITHUB_WORKSPACE/python/gtsam/tests
+  $PYTHON -m unittest discover -v
+}
 
-cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+# select between build or test
-    -DGTSAM_BUILD_TESTS=OFF \
+case $1 in
-    -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+  -d)
-    -DGTSAM_USE_QUATERNIONS=OFF \
+    install_dependencies
-    -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
+    ;;
-    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+  -b)
-    -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+    build
-    -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
+    ;;
-    -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
+  -t)
-    -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+    test
-    -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
+    ;;
-    -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+esac
-    -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
-
-
-# Set to 2 cores so that Actions does not error out during resource provisioning.
-make -j2 install
-
-cd $GITHUB_WORKSPACE/build/python
-$PYTHON -m pip install --user .
-cd $GITHUB_WORKSPACE/python/gtsam/tests
-$PYTHON -m unittest discover -v

.github/workflows/build-python.yml

@@ -117,11 +117,12 @@ jobs:
         uses: pierotofy/set-swap-space@master
         with:
           swap-size-gb: 6
-      - name: Build (Linux)
+      - name: Install Dependencies
-        if: runner.os == 'Linux'
         run: |
-          bash .github/scripts/python.sh
+          bash .github/scripts/python.sh -d
-      - name: Build (macOS)
+      - name: Build
-        if: runner.os == 'macOS'
         run: |
-          bash .github/scripts/python.sh
+          bash .github/scripts/python.sh -b
+      - name: Test
+        run: |
+          bash .github/scripts/python.sh -t

cmake/HandlePrintConfiguration.cmake

@@ -33,6 +33,7 @@ print_build_options_for_target(gtsam)
 
 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
 print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")
+print_config("Using Boost version" "${Boost_VERSION}")
 
 if(GTSAM_USE_TBB)
     print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")

gtsam/basis/basis.i

@@ -137,7 +137,7 @@ class FitBasis {
   static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
       const std::map<double, double>& sequence,
       const gtsam::noiseModel::Base* model, size_t N);
-  This::Parameters parameters() const;
+  gtsam::This::Parameters parameters() const;
 };
 
 }  // namespace gtsam

gtsam/geometry/geometry.i

@@ -1126,10 +1126,10 @@ class TriangulationResult {
   Status status;
   TriangulationResult(const gtsam::Point3& p);
   const gtsam::Point3& get() const;
-  static TriangulationResult Degenerate();
+  static gtsam::TriangulationResult Degenerate();
-  static TriangulationResult Outlier();
+  static gtsam::TriangulationResult Outlier();
-  static TriangulationResult FarPoint();
+  static gtsam::TriangulationResult FarPoint();
-  static TriangulationResult BehindCamera();
+  static gtsam::TriangulationResult BehindCamera();
   bool valid() const;
   bool degenerate() const;
   bool outlier() const;

gtsam/groups.dox

@@ -11,4 +11,7 @@
 
 @}
 
-*/ 
+\defgroup SLAM Useful SLAM components
+@{ @}
+
+*/

gtsam/gtsam.i

@@ -66,27 +66,6 @@ class KeySet {
   void serialize() const;
 };
 
-// Actually a vector<Key>
-class KeyVector {
-  KeyVector();
-  KeyVector(const gtsam::KeyVector& other);
-
-  // Note: no print function
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  size_t at(size_t i) const;
-  size_t front() const;
-  size_t back() const;
-  void push_back(size_t key) const;
-
-  void serialize() const;
-};
-
 // Actually a FastMap<Key,int>
 class KeyGroupMap {
   KeyGroupMap();

gtsam/navigation/CombinedImuFactor.h

@@ -123,7 +123,7 @@ public:
  * it is received from the IMU) so as to avoid costly integration at time of
  * factor construction.
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
 
@@ -252,7 +252,7 @@ public:
  *    the correlation between the bias uncertainty and the preintegrated
  *    measurements uncertainty.
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
     Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {

gtsam/navigation/ImuFactor.h

@@ -66,7 +66,7 @@ typedef ManifoldPreintegration PreintegrationType;
  * as soon as it is received from the IMU) so as to avoid costly integration
  * at time of factor construction.
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
@@ -165,7 +165,7 @@ public:
  * (which are usually slowly varying quantities), which is up to the caller.
  * See also CombinedImuFactor for a class that does this for you.
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
     imuBias::ConstantBias> {
@@ -256,7 +256,7 @@ public:
 
 /**
  * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
 private:

gtsam/nonlinear/CustomFactor.h

@@ -19,8 +19,6 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-using namespace gtsam;
-
 namespace gtsam {
 
 using JacobianVector = std::vector<Matrix>;

gtsam/nonlinear/GncOptimizer.h

@@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) {
 
 /* ************************************************************************* */
 template<class GncParameters>
-class GTSAM_EXPORT GncOptimizer {
+class GncOptimizer {
  public:
   /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
   typedef typename GncParameters::OptimizerType BaseOptimizer;

gtsam/nonlinear/GncParams.h

@@ -39,7 +39,7 @@ enum GncLossType {
 };
 
 template<class BaseOptimizerParameters>
-class GTSAM_EXPORT GncParams {
+class GncParams {
  public:
   /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
   typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
@@ -72,8 +72,14 @@ class GTSAM_EXPORT GncParams {
   double relativeCostTol = 1e-5;  ///< If relative cost change is below this threshold, stop iterating
   double weightsTol = 1e-4;  ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
   Verbosity verbosity = SILENT;  ///< Verbosity level
-  std::vector<size_t> knownInliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are inliers
+
-  std::vector<size_t> knownOutliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+  //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
+  /// Use IndexVector for inliers and outliers since it is fast + wrapping
+  using IndexVector = FastVector<uint64_t>;
+  ///< Slots in the factor graph corresponding to measurements that we know are inliers
+  IndexVector knownInliers = IndexVector();
+  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+  IndexVector knownOutliers = IndexVector();
 
   /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
   void setLossType(const GncLossType type) {
@@ -114,7 +120,7 @@ class GTSAM_EXPORT GncParams {
    * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
    * only apply GNC to prune outliers from the loop closures.
    * */
-  void setKnownInliers(const std::vector<size_t>& knownIn) {
+  void setKnownInliers(const IndexVector& knownIn) {
     for (size_t i = 0; i < knownIn.size(); i++){
       knownInliers.push_back(knownIn[i]);
     }
@@ -125,7 +131,7 @@ class GTSAM_EXPORT GncParams {
    * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
    * and you provide  knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
    * */
-  void setKnownOutliers(const std::vector<size_t>& knownOut) {
+  void setKnownOutliers(const IndexVector& knownOut) {
     for (size_t i = 0; i < knownOut.size(); i++){
       knownOutliers.push_back(knownOut[i]);
     }
@@ -163,7 +169,7 @@ class GTSAM_EXPORT GncParams {
       std::cout << "knownInliers: " << knownInliers[i] << "\n";
     for (size_t i = 0; i < knownOutliers.size(); i++)
       std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
-    baseOptimizerParams.print(str);
+    baseOptimizerParams.print("Base optimizer params: ");
   }
 };
 

gtsam/nonlinear/NonlinearFactor.cpp

@@ -167,8 +167,9 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
     return GaussianFactor::shared_ptr(
         new JacobianFactor(terms, b,
             boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
-  else
+  else {
     return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
+  }
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/PriorFactor.h

@@ -24,7 +24,7 @@ namespace gtsam {
 
   /**
    * A class for a soft prior on any Value type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class VALUE>
   class PriorFactor: public NoiseModelFactor1<VALUE> {

gtsam/nonlinear/custom.i

@@ -0,0 +1,38 @@
+//*************************************************************************
+// Custom Factor wrapping
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor : gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
+   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
+   * ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
+               const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "",
+             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+}

gtsam/nonlinear/nonlinear.i

@@ -99,11 +99,11 @@ class NonlinearFactorGraph {
   string dot(
       const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting());
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting());
   void saveGraph(
       const string& s, const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting()) const;
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
   Vector whitenedError(const gtsam::Values& x) const;
 };
 
-#include <gtsam/nonlinear/CustomFactor.h>
-virtual class CustomFactor : gtsam::NoiseModelFactor {
-  /*
-   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
-   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
-   * ignore list in `matlab/CMakeLists.txt`.
-   */
-  CustomFactor();
-  /*
-   * Example:
-   * ```
-   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
-   *    <calculated error>
-   *    if not H is None:
-   *        <calculate the Jacobian>
-   *        H[0] = J1 # 2-d numpy array for a Jacobian block
-   *        H[1] = J2
-   *        ...
-   *    return error # 1-d numpy array
-   *
-   * cf = CustomFactor(noise_model, keys, error_func)
-   * ```
-   */
-  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
-               const gtsam::KeyVector& keys,
-               const gtsam::CustomErrorFunction& errorFunction);
-
-  void print(string s = "",
-             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
-};
-
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();
@@ -544,12 +513,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
 };
 
 #include <gtsam/nonlinear/GncParams.h>
+enum GncLossType {
+  GM /*Geman McClure*/,
+  TLS /*Truncated least squares*/
+};
+
 template<PARAMS>
 virtual class GncParams {
   GncParams(const PARAMS& baseOptimizerParams);
   GncParams();
-  void setVerbosityGNC(const This::Verbosity value);
+  BaseOptimizerParameters baseOptimizerParams;
-  void print(const string& str) const;
+  gtsam::GncLossType lossType;
+  size_t maxIterations;
+  double muStep;
+  double relativeCostTol;
+  double weightsTol;
+  Verbosity verbosity;
+  gtsam::KeyVector knownInliers;
+  gtsam::KeyVector knownOutliers;
+
+  void setLossType(const gtsam::GncLossType type);
+  void setMaxIterations(const size_t maxIter);
+  void setMuStep(const double step);
+  void setRelativeCostTol(double value);
+  void setWeightsTol(double value);
+  void setVerbosityGNC(const gtsam::This::Verbosity value);
+  void setKnownInliers(const gtsam::KeyVector& knownIn);
+  void setKnownOutliers(const gtsam::KeyVector& knownOut);
+  void print(const string& str = "GncParams: ") const;
   
   enum Verbosity {
     SILENT,
@@ -597,6 +588,11 @@ virtual class GncOptimizer {
   GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
                const gtsam::Values& initialValues,
                const PARAMS& params);
+  void setInlierCostThresholds(const double inth);
+  const Vector& getInlierCostThresholds();
+  void setInlierCostThresholdsAtProbability(const double alpha);
+  void setWeights(const Vector w);
+  const Vector& getWeights();
   gtsam::Values optimize();
 };
 
@@ -705,7 +701,7 @@ class ISAM2Result {
   /** Getters and Setters for all properties */
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
-  FactorIndices getNewFactorsIndices() const;
+  gtsam::FactorIndices getNewFactorsIndices() const;
   size_t getCliques() const;
   double getErrorBefore() const;
   double getErrorAfter() const;
@@ -873,7 +869,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::PinholeCamera<gtsam::Cal3Unified>,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
-  NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
+  NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4);
   gtsam::Vector evaluateError(const T& x1, const T& x2);
 };
 

gtsam/sam/BearingRangeFactor.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * Binary factor for a bearing/range measurement
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template <typename A1, typename A2,
           typename B = typename Bearing<A1, A2>::result_type,

gtsam/slam/AntiFactor.h

@@ -26,7 +26,7 @@ namespace gtsam {
    * A class for downdating an existing factor from a graph. The AntiFactor returns the same
    * linearized Hessian matrices of the original factor, but with the opposite sign. This effectively
    * cancels out any affects of the original factor during optimization."
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   class AntiFactor: public NonlinearFactor {
 

gtsam/slam/BetweenFactor.h

@@ -34,7 +34,7 @@ namespace gtsam {
   /**
    * A class for a measurement predicted by "between(config[key1],config[key2])"
    * @tparam VALUE the Value type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class VALUE>
   class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {

gtsam/slam/BoundingConstraint.h

@@ -27,7 +27,7 @@ namespace gtsam {
  * greater/less than a fixed threshold.  The function
  * will need to have its value function implemented to return
  * a scalar for comparison.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<class VALUE>
 struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {

gtsam/slam/EssentialMatrixConstraint.h

@@ -25,7 +25,7 @@ namespace gtsam {
 
 /**
  * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
 

gtsam/slam/GeneralSFMFactor.h

@@ -54,7 +54,7 @@ namespace gtsam {
 /**
  * Non-linear factor for a constraint derived from a 2D measurement.
  * The calibration is unknown here compared to GenericProjectionFactor
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<class CAMERA, class LANDMARK>
 class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {

gtsam/slam/ProjectionFactor.h

@@ -33,7 +33,7 @@ namespace gtsam {
    * Non-linear factor for a constraint derived from a 2D measurement. 
    * The calibration is known here.
    * The main building block for visual SLAM.
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template <class POSE = Pose3, class LANDMARK = Point3,
             class CALIBRATION = Cal3_S2>

gtsam/slam/SmartProjectionPoseFactor.h

@@ -24,7 +24,7 @@
 namespace gtsam {
 /**
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
@@ -39,7 +39,7 @@ namespace gtsam {
  * The factor only constrains poses (variable dimension is 6).
  * This factor requires that values contains the involved poses (Pose3).
  * If the calibration should be optimized, as well, use SmartProjectionFactor instead!
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template <class CALIBRATION>
 class SmartProjectionPoseFactor

gtsam/slam/SmartProjectionRigFactor.h

@@ -29,7 +29,7 @@
 namespace gtsam {
 /**
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@@ -46,7 +46,7 @@ namespace gtsam {
  * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
  * instead! If the calibration should be optimized, as well, use
  * SmartProjectionFactor instead!
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template <class CAMERA>
 class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {

gtsam/slam/StereoFactor.h

@@ -25,7 +25,7 @@ namespace gtsam {
 
 /**
  * A Generic Stereo Factor
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<class POSE, class LANDMARK>
 class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {

gtsam/slam/TriangulationFactor.h

@@ -27,7 +27,7 @@ namespace gtsam {
 /**
  * Non-linear factor for a constraint derived from a 2D measurement.
  * The calibration and pose are assumed known.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<class CAMERA>
 class TriangulationFactor: public NoiseModelFactor1<Point3> {

gtsam_unstable/partition/tests/CMakeLists.txt

@@ -1,2 +1,2 @@
 set(ignore_test "testNestedDissection.cpp")
-gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam")
+gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if")

gtsam_unstable/slam/BetweenFactorEM.h

@@ -28,7 +28,7 @@ namespace gtsam {
 /**
  * A class for a measurement predicted by "between(config[key1],config[key2])"
  * @tparam VALUE the Value type
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<class VALUE>
 class BetweenFactorEM: public NonlinearFactor {

gtsam_unstable/slam/BiasedGPSFactor.h

@@ -25,7 +25,7 @@ namespace gtsam {
   /**
    * A class to model GPS measurements, including a bias term which models
    * common-mode errors and that can be partially corrected if other sensors are used
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
 

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -29,7 +29,7 @@ namespace gtsam {
   /**
    * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
    * i.e. the main building block for visual SLAM.
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
   class MultiProjectionFactor: public NoiseModelFactor {

gtsam_unstable/slam/PoseBetweenFactor.h

@@ -26,7 +26,7 @@ namespace gtsam {
   /**
    * A class for a measurement predicted by "between(config[key1],config[key2])"
    * @tparam POSE the Pose type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class POSE>
   class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {

gtsam_unstable/slam/PosePriorFactor.h

@@ -23,7 +23,7 @@ namespace gtsam {
 
   /**
    * A class for a soft prior on any Value type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class POSE>
   class PosePriorFactor: public NoiseModelFactor1<POSE> {

gtsam_unstable/slam/PoseToPointFactor.h

@@ -18,7 +18,7 @@ namespace gtsam {
 
 /**
  * A class for a measurement between a pose and a point.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template<typename POSE = Pose3, typename POINT = Point3>
 class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -28,7 +28,7 @@ namespace gtsam {
   /**
    * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
    * i.e. the main building block for visual SLAM.
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
   class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -30,7 +30,7 @@ namespace gtsam {
   /**
    * Non-linear factor for a constraint derived from a 2D measurement. This factor
    * estimates the body pose, body-camera transform, 3D landmark, and calibration.
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
 template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
 class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -38,7 +38,7 @@ namespace gtsam {
  * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
  * interpolated between A and B by the alpha to project the corresponding
  * landmark to Point2.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 
 class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -25,7 +25,7 @@
 namespace gtsam {
 /**
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@@ -39,7 +39,7 @@ namespace gtsam {
  * shutter model of the camera with given readout time. The factor requires that
  * values contain (for each pixel observation) two consecutive camera poses from
  * which the pixel observation pose can be interpolated.
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 template <class CAMERA>
 class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter

gtsam_unstable/slam/SmartRangeFactor.h

@@ -24,7 +24,7 @@ namespace gtsam {
 
 /**
  * Smart factor for range SLAM
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class SmartRangeFactor: public NoiseModelFactor {
  protected:

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -23,7 +23,7 @@
 namespace gtsam {
 /**
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@@ -38,7 +38,7 @@ namespace gtsam {
  * calibration or the same calibration can be shared by multiple cameras. This
  * factor requires that values contain the involved poses and extrinsics (both
  * are Pose3 variables).
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
     : public SmartStereoProjectionFactor {

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -26,7 +26,7 @@
 namespace gtsam {
 /**
  *
- * @addtogroup SLAM
+ * @ingroup SLAM
  *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
@@ -41,7 +41,7 @@ namespace gtsam {
  * has its own calibration.
  * The factor only constrains poses (variable dimension is 6).
  * This factor requires that values contains the involved poses (Pose3).
- * @addtogroup SLAM
+ * @ingroup SLAM
  */
 class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
     : public SmartStereoProjectionFactor {

gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h

@@ -29,7 +29,7 @@ namespace gtsam {
   /**
    * A class for a measurement predicted by "between(config[key1],config[key2])"
    * @tparam VALUE the Value type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class VALUE>
   class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -31,7 +31,7 @@ namespace gtsam {
   /**
    * A class for a measurement predicted by "between(config[key1],config[key2])"
    * @tparam VALUE the Value type
-   * @addtogroup SLAM
+   * @ingroup SLAM
    */
   template<class VALUE>
   class TransformBtwRobotsUnaryFactorEM: public NonlinearFactor {

matlab/CMakeLists.txt

@@ -73,6 +73,7 @@ set(interface_files
     ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
     ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i
     ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
     ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
     ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
@@ -98,7 +99,6 @@ endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
 
 # Record the root dir for gtsam - needed during external builds, e.g., ROS
 set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
-message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
 
 # Tests message(STATUS "Installing Matlab Toolbox")
 install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")

python/CMakeLists.txt

@@ -61,6 +61,7 @@ set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
     ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i
     ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
     ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
     ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i

python/gtsam/preamble/custom.h

@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */

python/gtsam/preamble/nonlinear.h

@@ -9,4 +9,4 @@
  * automatic STL binding, such that the raw objects can be accessed in Python.
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
- */
+ */

python/gtsam/specializations/custom.h

@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */

python/gtsam/specializations/nonlinear.h

@@ -9,4 +9,4 @@
  * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
  * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
  * and saves one copy operation.
- */
+ */

python/gtsam/tests/test_NonlinearOptimizer.py

@@ -15,10 +15,12 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
+from gtsam import (
-                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
+    DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
+    GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
-                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+    LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
+    PriorFactorPoint2, Values
+)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -27,7 +29,6 @@ KEY2 = 2
 
 class TestScenario(GtsamTestCase):
     """Do trivial test with three optimizer variants."""
-
     def setUp(self):
         """Set up the optimization problem and ordering"""
         # create graph
@@ -83,16 +84,83 @@ class TestScenario(GtsamTestCase):
         actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
         self.assertAlmostEqual(0, self.fg.error(actual))
 
+    def test_gnc_params(self):
+        base_params = LevenbergMarquardtParams()
+        # Test base params
+        for base_max_iters in (50, 100):
+            base_params.setMaxIterations(base_max_iters)
+            params = GncLMParams(base_params)
+            self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
+
+        # Test printing
+        params_str = str(params)
+        for s in (
+            "lossType",
+            "maxIterations",
+            "muStep",
+            "relativeCostTol",
+            "weightsTol",
+            "verbosity",
+        ):
+            self.assertTrue(s in params_str)
+
+        # Test each parameter
+        for loss_type in (GncLossType.TLS, GncLossType.GM):
+            params.setLossType(loss_type)  # Default is TLS
+            self.assertEqual(params.lossType, loss_type)
+        for max_iter in (1, 10, 100):
+            params.setMaxIterations(max_iter)
+            self.assertEqual(params.maxIterations, max_iter)
+        for mu_step in (1.1, 1.2, 1.5):
+            params.setMuStep(mu_step)
+            self.assertEqual(params.muStep, mu_step)
+        for rel_cost_tol in (1e-5, 1e-6, 1e-7):
+            params.setRelativeCostTol(rel_cost_tol)
+            self.assertEqual(params.relativeCostTol, rel_cost_tol)
+        for weights_tol in (1e-4, 1e-3, 1e-2):
+            params.setWeightsTol(weights_tol)
+            self.assertEqual(params.weightsTol, weights_tol)
+        for i in (0, 1, 2):
+            verb = GncLMParams.Verbosity(i)
+            params.setVerbosityGNC(verb)
+            self.assertEqual(params.verbosity, verb)
+        for inl in ([], [10], [0, 100]):
+            params.setKnownInliers(inl)
+            self.assertEqual(params.knownInliers, inl)
+            params.knownInliers = []
+        for out in ([], [1], [0, 10]):
+            params.setKnownInliers(out)
+            self.assertEqual(params.knownInliers, out)
+            params.knownInliers = []
+
+        # Test optimizer params
+        optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
+        for ict_factor in (0.9, 1.1):
+            new_ict = ict_factor * optimizer.getInlierCostThresholds()
+            optimizer.setInlierCostThresholds(new_ict)
+            self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
+        for w_factor in (0.8, 0.9):
+            new_weights = w_factor * optimizer.getWeights()
+            optimizer.setWeights(new_weights)
+            self.assertAlmostEqual(optimizer.getWeights(), new_weights)
+        optimizer.setInlierCostThresholdsAtProbability(0.9)
+        w1 = optimizer.getInlierCostThresholds()
+        optimizer.setInlierCostThresholdsAtProbability(0.8)
+        w2 = optimizer.getInlierCostThresholds()
+        self.assertLess(w2, w1)
+
     def test_iteration_hook(self):
         # set up iteration hook to track some testable values
         iteration_count = 0
         final_error = 0
         final_values = None
+
         def iteration_hook(iter, error_before, error_after):
             nonlocal iteration_count, final_error, final_values
             iteration_count = iter
             final_error = error_after
             final_values = optimizer.values()
+
         # optimize
         params = LevenbergMarquardtParams.CeresDefaults()
         params.setOrdering(self.ordering)
@@ -104,5 +172,6 @@ class TestScenario(GtsamTestCase):
         self.assertEqual(self.fg.error(actual), final_error)
         self.assertEqual(optimizer.iterations(), iteration_count)
 
+
 if __name__ == "__main__":
     unittest.main()

tests/testGncOptimizer.cpp

@@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
   // GNC
   // Note: in difficult instances, we set the odometry measurements to be
   // inliers, but this problem is simple enought to succeed even without that
-  // assumption std::vector<size_t> knownInliers;
+  // assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
   GncParams<GaussNewtonParams> gncParams;
   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
                                                         gncParams);
@@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
   // nonconvexity with known inliers and known outliers (check early stopping
   // when all measurements are known to be inliers or outliers)
   {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(0);
     knownInliers.push_back(1);
     knownInliers.push_back(2);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
 
   // nonconvexity with known inliers and known outliers
   {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(2);
     knownInliers.push_back(0);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
 
   // only known outliers
   {
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
   // initialize weights and also set known inliers/outliers
   {
     GncParams<GaussNewtonParams> gncParams;
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(2);
     knownInliers.push_back(0);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
     gncParams.setKnownInliers(knownInliers);
     gncParams.setKnownOutliers(knownOutliers);