Added negate() to GaussianFactor, which computes the Anti-factor eqiuvalent, using implementation from the nonlinear Anti-factor

gtsam/linear/GaussianFactor.h

@@ -115,6 +115,13 @@ namespace gtsam {
     	IndexFactor::permuteWithInverse(inversePermutation);
     }
 
+    /**
+     * Construct the corresponding anti-factor to negate information
+     * stored stored in this factor.
+     * @return a HessianFactor with negated Hessian matrices
+     */
+    virtual GaussianFactor::shared_ptr negate() const = 0;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/linear/HessianFactor.cpp

@@ -511,4 +511,24 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr
   return conditional;
 }
 
+/* ************************************************************************* */
+GaussianFactor::shared_ptr HessianFactor::negate() const {
+  // Copy Hessian Blocks from Hessian factor and invert
+  std::vector<Index> js;
+  std::vector<Matrix> Gs;
+  std::vector<Vector> gs;
+  double f;
+  js.insert(js.end(), begin(), end());
+  for(size_t i = 0; i < js.size(); ++i){
+    for(size_t j = i; j < js.size(); ++j){
+      Gs.push_back( -info(begin()+i, begin()+j) );
+    }
+    gs.push_back( -linearTerm(begin()+i) );
+  }
+  f = -constantTerm();
+
+  // Create the Anti-Hessian Factor from the negated blocks
+  return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
+}
+
 } // gtsam

gtsam/linear/HessianFactor.h

@@ -237,6 +237,13 @@ namespace gtsam {
     /** Return the number of columns and rows of the Hessian matrix */
     size_t rows() const { return info_.rows(); }
 
+    /**
+     * Construct the corresponding anti-factor to negate information
+     * stored stored in this factor.
+     * @return a HessianFactor with negated Hessian matrices
+     */
+    virtual GaussianFactor::shared_ptr negate() const;
+
     /** Return a view of the block at (j1,j2) of the <em>upper-triangular part</em> of the
      * information matrix \f$ H \f$, no data is copied.  See HessianFactor class documentation
      * above to explain that only the upper-triangular part of the information matrix is stored

gtsam/linear/JacobianFactor.cpp

@@ -385,6 +385,12 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  GaussianFactor::shared_ptr JacobianFactor::negate() const {
+  	HessianFactor hessian(*this);
+  	return hessian.negate();
+  }
+
   /* ************************************************************************* */
   GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() {
     return this->eliminate(1);

gtsam/linear/JacobianFactor.h

@@ -169,6 +169,13 @@ namespace gtsam {
      */
     virtual Matrix computeInformation() const;
 
+    /**
+     * Construct the corresponding anti-factor to negate information
+     * stored stored in this factor.
+     * @return a HessianFactor with negated Hessian matrices
+     */
+    virtual GaussianFactor::shared_ptr negate() const;
+
     /** Check if the factor contains no information, i.e. zero rows.  This does
      * not necessarily mean that the factor involves no variables (to check for
      * involving no variables use keys().empty()).

gtsam/slam/AntiFactor.h

@@ -99,30 +99,8 @@ namespace gtsam {
 	    // Generate the linearized factor from the contained nonlinear factor
 	    GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering);
 
-	    // Cast the GaussianFactor to a Hessian
-	    HessianFactor::shared_ptr hessianFactor = boost::dynamic_pointer_cast<HessianFactor>(gaussianFactor);
-
-	    // If the cast fails, convert it to a Hessian
-	    if(!hessianFactor){
-	      hessianFactor = HessianFactor::shared_ptr(new HessianFactor(*gaussianFactor));
-	    }
-
-	    // Copy Hessian Blocks from Hessian factor and invert
-	    std::vector<Index> js;
-	    std::vector<Matrix> Gs;
-	    std::vector<Vector> gs;
-	    double f;
-	    js.insert(js.end(), hessianFactor->begin(), hessianFactor->end());
-	    for(size_t i = 0; i < js.size(); ++i){
-	      for(size_t j = i; j < js.size(); ++j){
-	        Gs.push_back( -hessianFactor->info(hessianFactor->begin()+i, hessianFactor->begin()+j) );
-	      }
-	      gs.push_back( -hessianFactor->linearTerm(hessianFactor->begin()+i) );
-	    }
-	    f = -hessianFactor->constantTerm();
-
-	    // Create the Anti-Hessian Factor from the negated blocks
-	    return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
+	    // return the negated version of the factor
+	    return gaussianFactor->negate();
 	  }