From faadf5b06fab8537482aa87c190d23a4d5b69ea1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 6 Mar 2015 08:48:35 -0800
Subject: [PATCH] Fully compiles now

---
 gtsam/slam/JacobianFactorQ.h                         | 2 +-
 gtsam/slam/tests/testEssentialMatrixFactor.cpp       | 2 +-
 gtsam/slam/tests/testSmartProjectionCameraFactor.cpp | 8 ++++----
 gtsam_unstable/slam/SmartStereoProjectionFactor.h    | 9 +++++----
 4 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h
index 12f8a4c71..16560a43e 100644
--- a/gtsam/slam/JacobianFactorQ.h
+++ b/gtsam/slam/JacobianFactorQ.h
@@ -65,7 +65,7 @@ public:
     for (size_t k = 0; k < FBlocks.size(); ++k) {
       Key key = keys[k];
       QF.push_back(
-          KeyMatrix(key, Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
+          KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
     }
     // Which is then passed to the normal JacobianFactor constructor
     JacobianFactor::fillTerms(QF, - Q * b, model);
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index e0e26ecff..3bcc3eccd 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) {
 
     // Check evaluation
     Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
-    const Point2 pi = SimpleCamera::project_to_camera(P2);
+    const Point2 pi = PinholeBase::Project(P2);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index c7cf0283f..964f3bca4 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -160,10 +160,10 @@ TEST( SmartProjectionCameraFactor, noisy ) {
 
   // Check whitened errors
   Vector expected(4);
-  expected << -7, 235, 58, -242;
+  expected << 7, -235, -58, 242;
   SmartFactor::Cameras cameras1 = factor1->cameras(values);
   Point3 point1 = *factor1->point();
-  Vector actual = factor1->whitenedErrors(cameras1, point1);
+  Vector actual = factor1->whitenedError(cameras1, point1);
   EXPECT(assert_equal(expected, actual, 1));
 
   SmartFactor::shared_ptr factor2(new SmartFactor());
@@ -245,10 +245,10 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
 
   // Check whitened errors
   Vector expected(6);
-  expected << 256, 29, -26, 29, -206, -202;
+  expected << -256, -29, 26, -29, 206, 202;
   SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
   Point3 point1 = *smartFactor1->point();
-  Vector actual = smartFactor1->whitenedErrors(cameras1, point1);
+  Vector actual = smartFactor1->whitenedError(cameras1, point1);
   EXPECT(assert_equal(expected, actual, 1));
 
   // Optimize
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 65036edfe..5e0898bfa 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -415,7 +415,7 @@ public:
     std::vector<typename Base::MatrixZD> Fblocks;
     Matrix F, E;
     Vector b;
-    double f = computeJacobians(Fblocks, E, b, cameras);
+    computeJacobians(Fblocks, E, b, cameras);
     Base::FillDiagonalF(Fblocks, F); // expensive !!!
 
     // Schur complement trick
@@ -446,6 +446,7 @@ public:
       }
     }
     // ==================================================================
+    double f = b.squaredNorm();
     if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
       this->state_->Gs = Gs;
       this->state_->gs = gs;
@@ -549,7 +550,7 @@ public:
   /// Compute F, E only (called below in both vanilla and SVD versions)
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
-  double computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
+  void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
       Matrix& E, Vector& b, const Cameras& cameras) const {
     if (this->degenerate_) {
       throw("FIXME: computeJacobians degenerate case commented out!");
@@ -587,7 +588,7 @@ public:
 //      return f;
     } else {
       // nondegenerate: just return Base version
-      return Base::computeJacobians(Fblocks, E, b, cameras, point_);
+      Base::computeJacobians(Fblocks, E, b, cameras, point_);
     } // end else
   }
 
@@ -607,7 +608,7 @@ public:
     Cameras cameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
     if (nonDegenerate)
-      return Base::reprojectionError(cameras, point_);
+      return Base::unwhitenedError(cameras, point_);
     else
       return zero(cameras.size() * 3);
   }