diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index da8a0d976..26d9437a9 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -273,6 +273,24 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
 //        std::cout <<"  key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
 //      }
 
+      /////////////////////////////////////////////////////////////////
+      // PROTOTYPING
+      size_t numMeasurements = measured_.size();
+      Matrix F = Matrix::Zero(3*numMeasurements, 6 * nrUniqueKeys);
+      for(size_t k=0; k<numMeasurements; k++){
+        Key key_body = w_P_body_keys_.at(k);
+        Key key_cal = body_P_cam_keys_.at(k);
+        F.block<3,6>( 3*k , 6*keyToSlotMap[key_body] ) = Fs[k].block<3,6>(0,0);
+        F.block<3,6>( 3*k , 6*keyToSlotMap[key_cal] ) = Fs[k].block<3,6>(0,6);
+      }
+      Matrix augH = Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1);
+      augH.block(0,0,6*nrUniqueKeys,6*nrUniqueKeys) = F.transpose() * F - F.transpose() * E * P * E.transpose() * F;
+      Matrix infoVec = F.transpose() * b - F.transpose() * E * P * E.transpose() * b;
+      augH.block(0,6*nrUniqueKeys,6*nrUniqueKeys,1) = infoVec;
+      augH.block(6*nrUniqueKeys,0,1,6*nrUniqueKeys) = infoVec.transpose();
+      augH(6*nrUniqueKeys,6*nrUniqueKeys) = b.squaredNorm();
+      /////////////////////////////////////////////////////////////////
+
       // initialize matrix to zero
       augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
 
@@ -301,8 +319,13 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
       }
       augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
 
-      std::cout << "MAtrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
-      std::cout << "sq norm " << b.squaredNorm() << std::endl;
+      //std::cout << "Matrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
+      //std::cout << "sq norm " << b.squaredNorm() << std::endl;
+
+//      std::cout << "norm diff: "<< Matrix(augH - Matrix(augmentedHessianUniqueKeys.selfadjointView())) << std::endl;
+//
+//      std::cout << "TEST MATRIX:" << std::endl;
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, augH);
     }
 
     return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
index 228f5df77..e5d768dba 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
@@ -745,6 +745,54 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameEx
   EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7);
 }
 
+/* *************************************************************************/
+//TEST( SmartStereoProjectionFactorPP, smart_projection_factor_linearize_sameExtrinsicKey ) {
+//
+//  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+//  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+//  StereoCamera cam1(w_Pose_cam1, K2);
+//
+//  // create second camera 1 meter to the right of first camera
+//  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
+//  StereoCamera cam2(w_Pose_cam2, K2);
+//
+//  // create third camera 1 meter above the first camera
+//  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
+//  StereoCamera cam3(w_Pose_cam3, K2);
+//
+//  // three landmarks ~5 meters infront of camera
+//  Point3 landmark1(5, 0.5, 1.2);
+//
+//  // 1. Project three landmarks into three cameras and triangulate
+//  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
+//      cam2, cam3, landmark1);
+//
+//  KeyVector poseKeys;
+//  poseKeys.push_back(x1);
+//  poseKeys.push_back(x2);
+//  poseKeys.push_back(x3);
+//
+//  Symbol body_P_cam_key('P', 0);
+//  KeyVector extrinsicKeys;
+//  extrinsicKeys.push_back(body_P_cam_key);
+//  extrinsicKeys.push_back(body_P_cam_key);
+//  extrinsicKeys.push_back(body_P_cam_key);
+//
+//  SmartStereoProjectionParams smart_params;
+//  smart_params.triangulation.enableEPI = true;
+//  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
+//  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
+//
+//  // relevant poses:
+//  Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
+//  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
+//  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
+//  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
+//
+//  // Hessian
+//
+//}
+
 /* *************************************************************************/
 TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization_sameExtrinsicKey ) {
 
@@ -823,7 +871,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
 
   // cost is large before optimization
   double initialErrorSmart = graph.error(values);
-  EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
+  EXPECT_DOUBLES_EQUAL(31987.075556114589, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
 
   std::cout << " 1: " << smartFactor1->error(values) <<std::endl;
   std::cout << " 2: " << smartFactor2->error(values) <<std::endl;