From 2dc908c98613bf29d40571670403955047b3354d Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Fri, 19 Mar 2021 23:09:26 -0400
Subject: [PATCH] working on new sym matrix

---
 gtsam_unstable/slam/SmartStereoProjectionFactorPP.h | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index 3cbb4af30..03ddba852 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -195,6 +195,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
     std::vector<Vector> gs(numKeys);
 
     std::cout <<"using my hessian!!!!!!!!!!1" << std::endl;
+    for(size_t i=0; i<numKeys;i++){
+      std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl;
+    }
 
     if (this->measured_.size() != cameras(values).size())
       throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
@@ -235,8 +238,13 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
     SymmetricBlockMatrix augmentedHessian = //
         Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
 
+    std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, 6);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessianPP(dims, augmentedHessian.full());
+
     return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
-                                                                augmentedHessian);
+                                                          augmentedHessianPP);
   }
 
   /**