Gnc

gtsam/nonlinear/NonlinearFactor.cpp

@@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
               && noiseModel_->equals(*e->noiseModel_, tol)));
 }
 
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
+    const SharedNoiseModel newNoise) const {
+  NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast<NoiseModelFactor>(clone());
+  new_factor->noiseModel_ = newNoise;
+  return new_factor;
+}
+
 /* ************************************************************************* */
 static void check(const SharedNoiseModel& noiseModel, size_t m) {
   if (noiseModel && m != noiseModel->dim())

gtsam/nonlinear/NonlinearFactor.h

@@ -244,6 +244,12 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
+  /**
+   * Creates a shared_ptr clone of the
+   * factor with a new noise model
+   */
+  shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
+
  private:
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -113,6 +113,17 @@ class GTSAM_EXPORT NonlinearOptimizerParams {
 
   virtual void print(const std::string& str = "") const;
 
+  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
+    return maxIterations == other.getMaxIterations()
+        && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
+        && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
+        && std::abs(errorTol - other.getErrorTol()) <= tol
+        && verbosityTranslator(verbosity) == other.getVerbosity();
+    //  && orderingType.equals(other.getOrderingType()_;
+    // && linearSolverType == other.getLinearSolverType();
+    // TODO: check ordering, iterativeParams, and iterationsHook
+  }
+
   inline bool isMultifrontal() const {
     return (linearSolverType == MULTIFRONTAL_CHOLESKY)
         || (linearSolverType == MULTIFRONTAL_QR);

tests/smallExample.h

@@ -342,10 +342,25 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
     return (h(x) - z_);
   }
 
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+        return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+            gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
 };
 
 }
 
+/* ************************************************************************* */
+inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) {
+  using symbol_shorthand::X;
+  using symbol_shorthand::L;
+  boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
+  Point2 z(1.0, 0.0);
+  boost::shared_ptr<smallOptimize::UnaryFactor> factor(
+      new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
+  fg->push_back(factor);
+  return *fg;
+}
+
 /* ************************************************************************* */
 inline boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
   using symbol_shorthand::X;
@@ -363,6 +378,54 @@ inline NonlinearFactorGraph createReallyNonlinearFactorGraph() {
   return *sharedReallyNonlinearFactorGraph();
 }
 
+/* ************************************************************************* */
+inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
+  using symbol_shorthand::X;
+  boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
+  Point2 z(0.0, 0.0);
+  double sigma = 0.1;
+
+  boost::shared_ptr<PriorFactor<Point2>> factor(
+      new PriorFactor<Point2>(X(1), z, noiseModel::Isotropic::Sigma(2,sigma)));
+  // 3 noiseless inliers
+  fg->push_back(factor);
+  fg->push_back(factor);
+  fg->push_back(factor);
+
+  // 1 outlier
+  Point2 z_out(1.0, 0.0);
+  boost::shared_ptr<PriorFactor<Point2>> factor_out(
+      new PriorFactor<Point2>(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma)));
+  fg->push_back(factor_out);
+
+  return *fg;
+}
+
+/* ************************************************************************* */
+inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
+  using symbol_shorthand::X;
+  boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
+  Point2 z(0.0, 0.0);
+  double sigma = 0.1;
+  auto gmNoise = noiseModel::Robust::Create(
+            noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma));
+  boost::shared_ptr<PriorFactor<Point2>> factor(
+      new PriorFactor<Point2>(X(1), z, gmNoise));
+  // 3 noiseless inliers
+  fg->push_back(factor);
+  fg->push_back(factor);
+  fg->push_back(factor);
+
+  // 1 outlier
+  Point2 z_out(1.0, 0.0);
+  boost::shared_ptr<PriorFactor<Point2>> factor_out(
+      new PriorFactor<Point2>(X(1), z_out, gmNoise));
+  fg->push_back(factor_out);
+
+  return *fg;
+}
+
+
 /* ************************************************************************* */
 inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
   using namespace impl;