diff --git a/gtsam/Makefile b/gtsam/Makefile
index c16a361a2ed2efa06a37f502505203fba4185d7f..f298c38a2077df80bbc53997234b0a43dd6f81b8 100644
--- a/gtsam/Makefile
+++ b/gtsam/Makefile
@@ -5,7 +5,7 @@
DISTNAME= gtsam-3.1.0
EXTRACT_SUFX= .tgz
VERSION= 3.1.0
-PKGREVISION= 1
+PKGREVISION= 2
CATEGORIES= wip
MASTER_SITES= https://research.cc.gatech.edu/borg/sites/edu.borg/files/downloads/
diff --git a/gtsam/PLIST b/gtsam/PLIST
index cab70d58fe2c4f63f7894ee09b6639b67c949de9..85adb89886fa567a03b66d5b49b9f2aaf1aaa593 100644
--- a/gtsam/PLIST
+++ b/gtsam/PLIST
@@ -849,12 +849,14 @@ ${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearISAM.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearOptimizer.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearOptimizerParams.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/Ordering.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAOnlyAnchorsProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAOnlyAnchorsProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PASingleAnchorProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PASingleAnchorProjectionFactorCal3_S2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3OnlyAnchorsProjectionFactorCal3DS2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3OnlyAnchorsProjectionFactorCal3_S2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3ProjectionFactorCal3DS2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3ProjectionFactorCal3_S2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint2SingleAnchorProjectionFactorCal3DS2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint2SingleAnchorProjectionFactorCal3_S2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3SingleAnchorProjectionFactorCal3DS2.m
+${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3SingleAnchorProjectionFactorCal3_S2.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/ParallaxAnglePoint2.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/ParallaxAnglePoint3.m
${PLIST.toolbox}gtsam_toolbox/+gtsam/PinholeCameraCal3DS2.m
diff --git a/gtsam/distinfo b/gtsam/distinfo
index 5773f89d6ada3d6b4e15403ba77d3a291cb821fc..0b86a0f91299f173d50b87b5230cd20ee5280685 100644
--- a/gtsam/distinfo
+++ b/gtsam/distinfo
@@ -4,3 +4,4 @@ Size (gtsam-3.1.0.tgz) = 18228078 bytes
SHA1 (patch-aa) = 1390af9ce452d0585d13ebcab7f13ecf7fc8b4fc
SHA1 (patch-ab) = b0324c3061b99d060732f0911860c2fda146d50c
SHA1 (patch-ac) = 5cd8ee856125dfbcd7705b981a5bead62ac6a4ff
+SHA1 (patch-ad) = 61eacf5a77082c37bd557f0846a284e0a26afad4
diff --git a/gtsam/patches/patch-ad b/gtsam/patches/patch-ad
new file mode 100644
index 0000000000000000000000000000000000000000..52867bb19815caf6fd7f27cecca86d244dfe523a
--- /dev/null
+++ b/gtsam/patches/patch-ad
@@ -0,0 +1,161 @@
+Add error function from SingleAnchorProjectionFactor to ParallaxAnglePoint3
+--- gtsam.h 2015-05-07 14:21:16.768171500 +0200
++++ gtsam.h 2015-05-07 14:20:30.876077548 +0200
+@@ -2262,8 +2262,10 @@
+ // enabling serialization functionality
+ void serialize() const;
+ };
+-typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3_S2> PASingleAnchorProjectionFactorCal3_S2;
+-typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3DS2> PASingleAnchorProjectionFactorCal3DS2;
++typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3_S2> PAPoint2SingleAnchorProjectionFactorCal3_S2;
++typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3DS2> PAPoint2SingleAnchorProjectionFactorCal3DS2;
++typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3SingleAnchorProjectionFactorCal3_S2;
++typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3SingleAnchorProjectionFactorCal3DS2;
+
+
+ template<POSE, LANDMARK, CALIBRATION>
+@@ -2289,8 +2289,8 @@
+ // enabling serialization functionality
+ void serialize() const;
+ };
+-typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAOnlyAnchorsProjectionFactorCal3_S2;
+-typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAOnlyAnchorsProjectionFactorCal3DS2;
++typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3OnlyAnchorsProjectionFactorCal3_S2;
++typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3OnlyAnchorsProjectionFactorCal3DS2;
+
+
+ template<POSE, LANDMARK, CALIBRATION>
+@@ -2314,8 +2314,8 @@
+ // enabling serialization functionality
+ void serialize() const;
+ };
+-typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAProjectionFactorCal3_S2;
+-typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAProjectionFactorCal3DS2;
++typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3ProjectionFactorCal3_S2;
++typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3ProjectionFactorCal3DS2;
+
+
+ #include <gtsam/slam/GeneralSFMFactor.h>
+--- gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-07 14:21:16.768171500 +0200
++++ gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-07 14:11:25.325210470 +0200
+@@ -244,6 +244,120 @@
+ return ones(2) * 2.0 * K_->fx();
+ }
+
++ /// Evaluate error h(x)-z and optionally derivatives
++ Vector evaluateError(const Pose3& mainPose, const ParallaxAnglePoint3& point,
++ boost::optional<Matrix&> Dmain = boost::none,
++ boost::optional<Matrix&> Dpoint = boost::none) const {
++ try {
++ // Test if we need jacobians
++ if (!Dmain && !Dpoint)
++ {
++ if(body_P_sensor_)
++ {
++ // Get the main and associated anchors, and the camera pose
++ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_) );
++
++ // Get the direction to the point from observation point
++ Point3 obs_T_point( point.directionVectorFromMainAnchor() );
++
++ // Put a camera at the origin
++ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
++
++ // Project direction vector to camera and calculate the error
++ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
++ return reprojectionError.vector();
++ }
++ else
++ {
++ // Get the direction to the point from observation point
++ Point3 obs_T_point( point.directionVectorFromMainAnchor() );
++
++ // Put a camera at the origin
++ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
++
++ // Project direction vector to camera and calculate the error
++ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
++ return reprojectionError.vector();
++ }
++ }
++
++ // Same computation but with jacobians
++ if(body_P_sensor_)
++ {
++ // Get the main and associated anchors, and the camera pose
++ gtsam::Matrix MAINANCHORPOSE_mainpose;
++ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_, MAINANCHORPOSE_mainpose) );
++
++ // Get the direction to the point from observation point
++ Matrix OBS_T_POINT_point;
++ Point3 obs_T_point(point.directionVectorFromMainAnchor( OBS_T_POINT_point ));
++
++ // Put a camera at the origin
++ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
++
++ // Project direction vector to camera and calculate the error
++ Matrix PROJ_mainanchorori, PROJ_obs_t_point;
++ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainanchorori, PROJ_obs_t_point) - measured_);
++
++ // Chain of jacobians
++ if(Dmain)
++ {
++ Matrix PROJ_mainanchorpose(2,6);
++ PROJ_mainanchorpose << PROJ_mainanchorori.block(0,0,2,3), zeros(2,3);
++ Dmain->resize(2,6);
++ *Dmain << (PROJ_mainanchorpose * MAINANCHORPOSE_mainpose);
++ }
++ if(Dpoint)
++ {
++ Dpoint->resize(2,3);
++ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
++ }
++
++ return reprojectionError.vector();
++
++ }
++ else
++ {
++ // Get the direction to the point from observation point
++ Matrix OBS_T_POINT_point;
++ Point3 obs_T_point(point.directionVectorFromMainAnchor( OBS_T_POINT_point ));
++
++ // Put a camera at the origin
++ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
++
++ // Project direction vector to camera and calculate the error
++ Matrix PROJ_mainori, PROJ_obs_t_point;
++ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainori, PROJ_obs_t_point) - measured_);
++
++ // Chain of jacobians
++ if(Dmain)
++ {
++ Dmain->resize(2,6);
++ *Dmain << PROJ_mainori.block(0,0,2,3), zeros(2,3);
++ }
++ if(Dpoint)
++ {
++ Dpoint->resize(2,3);
++ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
++ }
++
++ return reprojectionError.vector();
++ }
++ } catch( CheiralityException& e)
++ {
++ if (Dmain ) *Dmain = zeros(2,6);
++ if (Dpoint) *Dpoint = zeros(2,3);
++ if (verboseCheirality_)
++ std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
++ " with single anchor (" << DefaultKeyFormatter(this->key1()) << ")" <<
++ " moved behind camera " << std::endl;
++ if (throwCheirality_)
++ throw e;
++ }
++ return ones(2) * 2.0 * K_->fx();
++ }
++
++
+ /** return the measurement */
+ const Point2& measured() const {
+ return measured_;