Merge branch 'develop' into feature/discrete_wrapper

.gitignore

@@ -3,6 +3,7 @@
 .idea
 *.pyc
 *.DS_Store
+*.swp
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
 /examples/Data/pose2example-rewritten.txt
 /examples/Data/pose3example-rewritten.txt

CMakeLists.txt

@@ -105,6 +105,11 @@ endif()
 GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
 export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
 
+if (GTSAM_BUILD_UNSTABLE)
+    GtsamMakeConfigFile(GTSAM_UNSTABLE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
+    export(TARGETS ${GTSAM_UNSTABLE_EXPORTED_TARGETS} FILE GTSAM_UNSTABLE-exports.cmake)
+endif()
+
 # Check for doxygen availability - optional dependency
 find_package(Doxygen)
 

cmake/GtsamMakeConfigFile.cmake

@@ -27,6 +27,8 @@ function(GtsamMakeConfigFile PACKAGE_NAME)
 	# here.
 	if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING)
 		set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING})
+	elseif(NOT DEFINED ${PACKAGE_NAME}_VERSION_STRING)
+		set(${PACKAGE_NAME}_VERSION ${GTSAM_VERSION_STRING})
 	endif()
 
 	# Version file

cmake/HandleGeneralOptions.cmake

@@ -28,6 +28,7 @@ option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V41     "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
 option(GTSAM_SUPPORT_NESTED_DISSECTION      "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration on tangent space" ON)
+option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
 if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()

cmake/HandleTBB.cmake

@@ -7,9 +7,9 @@ if (GTSAM_WITH_TBB)
     if(TBB_FOUND)
         set(GTSAM_USE_TBB 1)  # This will go into config.h
 
-        if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1"))
+#        if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1"))
-            message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.")
+#            message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.")
-        endif()
+#        endif()
 
         if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
             set(TBB_GREATER_EQUAL_2020 1)

doc/math.lyx

@@ -5082,6 +5082,394 @@ reference "ex:projection"
 \end_inset
 
 
+\end_layout
+
+\begin_layout Subsection
+Derivative of Adjoint
+\begin_inset CommandInset label
+LatexCommand label
+name "subsec:pose3_adjoint_deriv"
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Consider 
+\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$
+\end_inset
+
+ is defined as 
+\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$
+\end_inset
+
+.
+ The derivative is notated (see Section 
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "sec:Derivatives-of-Actions"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+):
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b})
+\]
+
+\end_inset
+
+First, computing 
+\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$
+\end_inset
+
+ is easy, as its matrix is simply 
+\begin_inset Formula $Ad_{T}$
+\end_inset
+
+:
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b})
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T}
+\]
+
+\end_inset
+
+We will derive 
+\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$
+\end_inset
+
+ using two approaches.
+ In the first, we'll define 
+\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$
+\end_inset
+
+.
+ From Section 
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "sec:Derivatives-of-Actions"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+,
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{align*}
+D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\
+D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1}
+\end{align*}
+
+\end_inset
+
+Now we can use the definition of the Adjoint representation 
+\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$
+\end_inset
+
+ (aka conjugation by 
+\begin_inset Formula $g$
+\end_inset
+
+) then apply product rule and simplify:
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{align*}
+D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\
+ & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\
+ & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\
+ & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\
+ & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\
+ & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\
+D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}})
+\end{align*}
+
+\end_inset
+
+Where 
+\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$
+\end_inset
+
+ is the adjoint map of the lie algebra.
+\end_layout
+
+\begin_layout Standard
+The second, perhaps more intuitive way of deriving 
+\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$
+\end_inset
+
+, would be to use the fact that the derivative at the origin 
+\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$
+\end_inset
+
+ by definition of the adjoint 
+\begin_inset Formula $ad_{\xi}$
+\end_inset
+
+.
+ Then applying the property 
+\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$
+\end_inset
+
+,
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right)
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsection
+Derivative of AdjointTranspose
+\end_layout
+
+\begin_layout Standard
+The transpose of the Adjoint, 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\xout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$
+\end_inset
+
+, is useful as a way to change the reference frame of vectors in the dual
+ space 
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\strikeout default
+\xout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+(note the 
+\begin_inset Formula $^{*}$
+\end_inset
+
+ denoting that we are now in the dual space)
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\xout off
+\uuline off
+\uwave off
+\noun off
+\color none
+.
+ To be more concrete, where
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\strikeout default
+\xout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+as 
+\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$
+\end_inset
+
+ converts the 
+\emph on
+twist
+\emph default
+ 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\xout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula $\xi_{b}$
+\end_inset
+
+ from the 
+\begin_inset Formula $T$
+\end_inset
+
+ frame,
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\strikeout default
+\xout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+ 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\xout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
+\end_inset
+
+ converts the 
+\family default
+\series default
+\shape default
+\size default
+\emph on
+\bar default
+\strikeout default
+\xout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+wrench
+\emph default
+ 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\xout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula $\xi_{b}^{*}$
+\end_inset
+
+ from the 
+\begin_inset Formula $T$
+\end_inset
+
+ frame
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\strikeout default
+\xout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+.
+ It's difficult to apply a similar derivation as in Section 
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "subsec:pose3_adjoint_deriv"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ for the derivative of 
+\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
+\end_inset
+
+ because 
+\begin_inset Formula $Ad_{T}^{T}$
+\end_inset
+
+ cannot be naturally defined as a conjugation, so we resort to crunching
+ through the algebra.
+ The details are omitted but the result is a form that vaguely resembles
+ (but does not exactly match) 
+\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$
+\end_inset
+
+:
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{align*}
+\begin{bmatrix}\omega_{T}\\
+v_{T}
+\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\
+D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\
+\hat{v}_{T} & 0
+\end{bmatrix}
+\end{align*}
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Subsection

examples/DiscreteBayesNetExample.cpp

@@ -56,8 +56,8 @@ int main(int argc, char **argv) {
   DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
 
   // solve
-  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  auto mpe = chordal->optimize();
-  GTSAM_PRINT(*mpe);
+  GTSAM_PRINT(mpe);
 
   // We can also build a Bayes tree (directed junction tree).
   // The elimination order above will do fine:
@@ -70,14 +70,14 @@ int main(int argc, char **argv) {
 
   // solve again, now with evidence
   DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
-  DiscreteFactor::sharedValues mpe2 = chordal2->optimize();
+  auto mpe2 = chordal2->optimize();
-  GTSAM_PRINT(*mpe2);
+  GTSAM_PRINT(mpe2);
 
   // We can also sample from it
   cout << "\n10 samples:" << endl;
   for (size_t i = 0; i < 10; i++) {
-    DiscreteFactor::sharedValues sample = chordal2->sample();
+    auto sample = chordal2->sample();
-    GTSAM_PRINT(*sample);
+    GTSAM_PRINT(sample);
   }
   return 0;
 }

examples/DiscreteBayesNet_FG.cpp

@@ -33,11 +33,11 @@ using namespace gtsam;
 int main(int argc, char **argv) {
   // Define keys and a print function
   Key C(1), S(2), R(3), W(4);
-  auto print = [=](DiscreteFactor::sharedValues values) {
+  auto print = [=](const DiscreteFactor::Values& values) {
-    cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C])
+    cout << boolalpha << "Cloudy = " << static_cast<bool>(values.at(C))
-         << "  Sprinkler = " << static_cast<bool>((*values)[S])
+         << "  Sprinkler = " << static_cast<bool>(values.at(S))
-         << "  Rain = " << boolalpha << static_cast<bool>((*values)[R])
+         << "  Rain = " << boolalpha << static_cast<bool>(values.at(R))
-         << "  WetGrass = " << static_cast<bool>((*values)[W]) << endl;
+         << "  WetGrass = " << static_cast<bool>(values.at(W)) << endl;
   };
 
   // We assume binary state variables
@@ -85,7 +85,7 @@ int main(int argc, char **argv) {
         }
 
   // "Most Probable Explanation", i.e., configuration with largest value
-  DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize();
+  auto mpe = graph.eliminateSequential()->optimize();
   cout << "\nMost Probable Explanation (MPE):" << endl;
   print(mpe);
 
@@ -97,7 +97,7 @@ int main(int argc, char **argv) {
 
   // solve again, now with evidence
   DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize();
+  auto mpe_with_evidence = chordal->optimize();
 
   cout << "\nMPE given C=0:" << endl;
   print(mpe_with_evidence);
@@ -113,7 +113,7 @@ int main(int argc, char **argv) {
   // We can also sample from it
   cout << "\n10 samples:" << endl;
   for (size_t i = 0; i < 10; i++) {
-    DiscreteFactor::sharedValues sample = chordal->sample();
+    auto sample = chordal->sample();
     print(sample);
   }
   return 0;

examples/HMMExample.cpp

@@ -66,14 +66,14 @@ int main(int argc, char **argv) {
   chordal->print("Eliminated");
 
   // solve
-  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  auto mpe = chordal->optimize();
-  GTSAM_PRINT(*mpe);
+  GTSAM_PRINT(mpe);
 
   // We can also sample from it
   cout << "\n10 samples:" << endl;
   for (size_t k = 0; k < 10; k++) {
-    DiscreteFactor::sharedValues sample = chordal->sample();
+    auto sample = chordal->sample();
-    GTSAM_PRINT(*sample);
+    GTSAM_PRINT(sample);
   }
 
   // Or compute the marginals. This re-eliminates the FG into a Bayes tree

examples/IMUKittiExampleGPS.cpp

@@ -11,21 +11,23 @@
 
 /**
  * @file IMUKittiExampleGPS
- * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
+ * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
- * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
+ * VISION BENCHMARK SUITE
+ * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
+ * Electronics
  */
 
 // GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/slam/dataset.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/ISAM2Params.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
 
 #include <cstring>
 #include <fstream>
@@ -34,9 +36,9 @@
 using namespace std;
 using namespace gtsam;
 
-using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
-using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
 
 struct KittiCalibration {
   double body_ptx;
@@ -73,9 +75,11 @@ void loadKittiData(KittiCalibration& kitti_calibration,
   string line;
 
   // Read IMU metadata and compute relative sensor pose transforms
-    // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
+  // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
-    // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
+  // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
-    string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
+  // AverageDeltaT
+  string imu_metadata_file =
+      findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
   ifstream imu_metadata(imu_metadata_file.c_str());
 
   printf("-- Reading sensor metadata\n");
@@ -85,12 +89,9 @@ void loadKittiData(KittiCalibration& kitti_calibration,
   // Load Kitti calibration
   getline(imu_metadata, line, '\n');
   sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
-           &kitti_calibration.body_ptx,
+         &kitti_calibration.body_ptx, &kitti_calibration.body_pty,
-           &kitti_calibration.body_pty,
+         &kitti_calibration.body_ptz, &kitti_calibration.body_prx,
-           &kitti_calibration.body_ptz,
+         &kitti_calibration.body_pry, &kitti_calibration.body_prz,
-           &kitti_calibration.body_prx,
-           &kitti_calibration.body_pry,
-           &kitti_calibration.body_prz,
          &kitti_calibration.accelerometer_sigma,
          &kitti_calibration.gyroscope_sigma,
          &kitti_calibration.integration_sigma,
@@ -98,15 +99,11 @@ void loadKittiData(KittiCalibration& kitti_calibration,
          &kitti_calibration.gyroscope_bias_sigma,
          &kitti_calibration.average_delta_t);
   printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
-           kitti_calibration.body_ptx,
+         kitti_calibration.body_ptx, kitti_calibration.body_pty,
-           kitti_calibration.body_pty,
+         kitti_calibration.body_ptz, kitti_calibration.body_prx,
-           kitti_calibration.body_ptz,
+         kitti_calibration.body_pry, kitti_calibration.body_prz,
-           kitti_calibration.body_prx,
-           kitti_calibration.body_pry,
-           kitti_calibration.body_prz,
          kitti_calibration.accelerometer_sigma,
-           kitti_calibration.gyroscope_sigma,
+         kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
-           kitti_calibration.integration_sigma,
          kitti_calibration.accelerometer_bias_sigma,
          kitti_calibration.gyroscope_bias_sigma,
          kitti_calibration.average_delta_t);
@@ -119,13 +116,12 @@ void loadKittiData(KittiCalibration& kitti_calibration,
     ifstream imu_data(imu_data_file.c_str());
     getline(imu_data, line, '\n');  // ignore the first line
 
-        double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
+    double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0,
+           gyro_y = 0, gyro_z = 0;
     while (!imu_data.eof()) {
       getline(imu_data, line, '\n');
-            sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
+      sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
-                   &time, &dt,
+             &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
-                   &acc_x, &acc_y, &acc_z,
-                   &gyro_x, &gyro_y, &gyro_z);
 
       ImuMeasurement measurement;
       measurement.time = time;
@@ -163,12 +159,16 @@ int main(int argc, char* argv[]) {
   vector<GpsMeasurement> gps_measurements;
   loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
 
-    Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
+  Vector6 BodyP =
-                                  kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
+      (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
+       kitti_calibration.body_ptz, kitti_calibration.body_prx,
+       kitti_calibration.body_pry, kitti_calibration.body_prz)
           .finished();
   auto body_T_imu = Pose3::Expmap(BodyP);
   if (!body_T_imu.equals(Pose3(), 1e-5)) {
-        printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
+    printf(
+        "Currently only support IMUinBody is identity, i.e. IMU and body frame "
+        "are the same");
     exit(-1);
   }
 
@@ -180,38 +180,45 @@ int main(int argc, char* argv[]) {
   auto w_coriolis = Vector3::Zero();  // zero vector
 
   // Configure noise models
-    auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+  auto noise_model_gps = noiseModel::Diagonal::Precisions(
-                                                                          Vector3::Constant(1.0/0.07))
+      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
           .finished());
 
   // Set initial conditions for the estimated trajectory
   // initial pose is the reference frame (navigation frame)
-    auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
+  auto current_pose_global =
+      Pose3(Rot3(), gps_measurements[first_gps_pose].position);
   // the vehicle is stationary at the beginning at position 0,0,0
   Vector3 current_velocity_global = Vector3::Zero();
   auto current_bias = imuBias::ConstantBias();  // init with zero bias
 
-    auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+  auto sigma_init_x = noiseModel::Diagonal::Precisions(
-                                                                       Vector3::Constant(1.0))
+      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
-                                                         .finished());
   auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
-    auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
+  auto sigma_init_b = noiseModel::Diagonal::Sigmas(
-                                                                   Vector3::Constant(5.00e-05))
+      (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
           .finished());
 
   // Set IMU preintegration parameters
-    Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
+  Matrix33 measured_acc_cov =
-    Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
+      I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
+  Matrix33 measured_omega_cov =
+      I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
   // error committed in integrating position from velocities
-    Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
+  Matrix33 integration_error_cov =
+      I_3x3 * pow(kitti_calibration.integration_sigma, 2);
 
   auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
-    imu_params->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
+  imu_params->accelerometerCovariance =
-    imu_params->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
+      measured_acc_cov;  // acc white noise in continuous
-    imu_params->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous
+  imu_params->integrationCovariance =
+      integration_error_cov;  // integration uncertainty continuous
+  imu_params->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
   imu_params->omegaCoriolis = w_coriolis;
 
-    std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
+  std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
+      nullptr;
 
   // Set ISAM2 parameters and create ISAM2 solver object
   ISAM2Params isam_params;
@@ -220,16 +227,22 @@ int main(int argc, char* argv[]) {
 
   ISAM2 isam(isam_params);
 
-    // Create the factor graph and values object that will store new factors and values to add to the incremental graph
+  // Create the factor graph and values object that will store new factors and
+  // values to add to the incremental graph
   NonlinearFactorGraph new_factors;
-    Values new_values;  // values storing the initial estimates of new nodes in the factor graph
+  Values new_values;  // values storing the initial estimates of new nodes in
+                      // the factor graph
 
   /// Main loop:
   /// (1) we read the measurements
   /// (2) we create the corresponding factors in the graph
   /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
-    printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
+  printf(
+      "-- Starting main loop: inference is performed at each time step, but we "
+      "plot trajectory every 10 steps\n");
   size_t j = 0;
+  size_t included_imu_measurement_count = 0;
+
   for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
     // At each non=IMU measurement we initialize a new node in the graph
     auto current_pose_key = X(i);
@@ -242,19 +255,24 @@ int main(int argc, char* argv[]) {
       new_values.insert(current_pose_key, current_pose_global);
       new_values.insert(current_vel_key, current_velocity_global);
       new_values.insert(current_bias_key, current_bias);
-            new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
+      new_factors.emplace_shared<PriorFactor<Pose3>>(
-            new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
+          current_pose_key, current_pose_global, sigma_init_x);
-            new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
+      new_factors.emplace_shared<PriorFactor<Vector3>>(
+          current_vel_key, current_velocity_global, sigma_init_v);
+      new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
+          current_bias_key, current_bias, sigma_init_b);
     } else {
       double t_previous = gps_measurements[i - 1].time;
 
       // Summarize IMU data between the previous GPS measurement and now
-            current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
+      current_summarized_measurement =
-            static size_t included_imu_measurement_count = 0;
+          std::make_shared<PreintegratedImuMeasurements>(imu_params,
+                                                         current_bias);
+
       while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
         if (imu_measurements[j].time >= t_previous) {
-                    current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
+          current_summarized_measurement->integrateMeasurement(
-                                                                         imu_measurements[j].gyroscope,
+              imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
               imu_measurements[j].dt);
           included_imu_measurement_count++;
         }
@@ -266,34 +284,40 @@ int main(int argc, char* argv[]) {
       auto previous_vel_key = V(i - 1);
       auto previous_bias_key = B(i - 1);
 
-            new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
+      new_factors.emplace_shared<ImuFactor>(
-                                                  current_pose_key, current_vel_key,
+          previous_pose_key, previous_vel_key, current_pose_key,
-                                                  previous_bias_key, *current_summarized_measurement);
+          current_vel_key, previous_bias_key, *current_summarized_measurement);
 
       // Bias evolution as given in the IMU metadata
-            auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
+      auto sigma_between_b = noiseModel::Diagonal::Sigmas(
-                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
+          (Vector6() << Vector3::Constant(
-                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
+               sqrt(included_imu_measurement_count) *
+               kitti_calibration.accelerometer_bias_sigma),
+           Vector3::Constant(sqrt(included_imu_measurement_count) *
+                             kitti_calibration.gyroscope_bias_sigma))
               .finished());
-            new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
+      new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
-                                                                             current_bias_key,
+          previous_bias_key, current_bias_key, imuBias::ConstantBias(),
-                                                                             imuBias::ConstantBias(),
           sigma_between_b);
 
       // Create GPS factor
-            auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
+      auto gps_pose =
+          Pose3(current_pose_global.rotation(), gps_measurements[i].position);
       if ((i % gps_skip) == 0) {
-                new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
+        new_factors.emplace_shared<PriorFactor<Pose3>>(
+            current_pose_key, gps_pose, noise_model_gps);
         new_values.insert(current_pose_key, gps_pose);
 
-                printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
+        printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
+               t);
         cout << gps_pose.translation();
         printf("\n\n");
       } else {
         new_values.insert(current_pose_key, current_pose_global);
       }
 
-            // Add initial values for velocity and bias based on the previous estimates
+      // Add initial values for velocity and bias based on the previous
+      // estimates
       new_values.insert(current_vel_key, current_velocity_global);
       new_values.insert(current_bias_key, current_bias);
 
@@ -302,7 +326,8 @@ int main(int argc, char* argv[]) {
       // We accumulate 2*GPSskip GPS measurements before updating the solver at
       // first so that the heading becomes observable.
       if (i > (first_gps_pose + 2 * gps_skip)) {
-                printf("################ NEW FACTORS AT TIME %lf ################\n", t);
+        printf("############ NEW FACTORS AT TIME %.6lf ############\n",
+               t);
         new_factors.print();
 
         isam.update(new_factors, new_values);
@@ -318,7 +343,7 @@ int main(int argc, char* argv[]) {
         current_velocity_global = result.at<Vector3>(current_vel_key);
         current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
 
-                printf("\n################ POSE AT TIME %lf ################\n", t);
+        printf("\n############ POSE AT TIME %lf ############\n", t);
         current_pose_global.print();
         printf("\n\n");
       }
@@ -328,7 +353,8 @@ int main(int argc, char* argv[]) {
   // Save results to file
   printf("\nWriting results to file...\n");
   FILE* fp_out = fopen(output_filename.c_str(), "w+");
-    fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
+  fprintf(fp_out,
+          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
 
   Values result = isam.calculateEstimate();
   for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
@@ -349,10 +375,9 @@ int main(int argc, char* argv[]) {
     cout << "Bias:" << endl << bias << endl;
 
     fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
-                gps_measurements[i].time,
+            gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
-                pose.x(), pose.y(), pose.z(),
+            pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
-                pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
+            gps(1), gps(2));
-                gps(0), gps(1), gps(2));
   }
 
   fclose(fp_out);

examples/UGM_chain.cpp

@@ -70,8 +70,8 @@ int main(int argc, char** argv) {
   // "Decoding", i.e., configuration with largest value
   // We use sequential variable elimination
   DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
+  auto optimalDecoding = chordal->optimize();
-  optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
+  optimalDecoding.print("\nMost Probable Explanation (optimalDecoding)\n");
 
   // "Inference" Computing marginals for each node
   // Here we'll make use of DiscreteMarginals class, which makes use of

examples/UGM_small.cpp

@@ -63,8 +63,8 @@ int main(int argc, char** argv) {
   // "Decoding", i.e., configuration with largest value (MPE)
   // We use sequential variable elimination
   DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
+  auto optimalDecoding = chordal->optimize();
-  optimalDecoding->print("\noptimalDecoding");
+  GTSAM_PRINT(optimalDecoding);
 
   // "Inference" Computing marginals
   cout << "\nComputing Node Marginals .." << endl;

gtsam/base/Lie.h

@@ -370,4 +370,4 @@ public:
  * the gtsam namespace to be more easily enforced as testable
  */
 #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
-#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;
+#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;

gtsam/base/Manifold.h

@@ -178,4 +178,4 @@ struct FixedDimension {
 // * the gtsam namespace to be more easily enforced as testable
 // */
 #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
-#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;
+#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;

gtsam/base/Matrix.h

@@ -46,28 +46,28 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
 // Create handy typedefs and constants for square-size matrices
 // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
 #define GTSAM_MAKE_MATRIX_DEFS(N)   \
-typedef Eigen::Matrix<double, N, N> Matrix##N;  \
+using Matrix##N = Eigen::Matrix<double, N, N>;  \
-typedef Eigen::Matrix<double, 1, N> Matrix1##N;  \
+using Matrix1##N = Eigen::Matrix<double, 1, N>;  \
-typedef Eigen::Matrix<double, 2, N> Matrix2##N;  \
+using Matrix2##N = Eigen::Matrix<double, 2, N>;  \
-typedef Eigen::Matrix<double, 3, N> Matrix3##N;  \
+using Matrix3##N = Eigen::Matrix<double, 3, N>;  \
-typedef Eigen::Matrix<double, 4, N> Matrix4##N;  \
+using Matrix4##N = Eigen::Matrix<double, 4, N>;  \
-typedef Eigen::Matrix<double, 5, N> Matrix5##N;  \
+using Matrix5##N = Eigen::Matrix<double, 5, N>;  \
-typedef Eigen::Matrix<double, 6, N> Matrix6##N;  \
+using Matrix6##N = Eigen::Matrix<double, 6, N>;  \
-typedef Eigen::Matrix<double, 7, N> Matrix7##N;  \
+using Matrix7##N = Eigen::Matrix<double, 7, N>;  \
-typedef Eigen::Matrix<double, 8, N> Matrix8##N;  \
+using Matrix8##N = Eigen::Matrix<double, 8, N>;  \
-typedef Eigen::Matrix<double, 9, N> Matrix9##N;  \
+using Matrix9##N = Eigen::Matrix<double, 9, N>;  \
 static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
 static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
 
-GTSAM_MAKE_MATRIX_DEFS(1);
+GTSAM_MAKE_MATRIX_DEFS(1)
-GTSAM_MAKE_MATRIX_DEFS(2);
+GTSAM_MAKE_MATRIX_DEFS(2)
-GTSAM_MAKE_MATRIX_DEFS(3);
+GTSAM_MAKE_MATRIX_DEFS(3)
-GTSAM_MAKE_MATRIX_DEFS(4);
+GTSAM_MAKE_MATRIX_DEFS(4)
-GTSAM_MAKE_MATRIX_DEFS(5);
+GTSAM_MAKE_MATRIX_DEFS(5)
-GTSAM_MAKE_MATRIX_DEFS(6);
+GTSAM_MAKE_MATRIX_DEFS(6)
-GTSAM_MAKE_MATRIX_DEFS(7);
+GTSAM_MAKE_MATRIX_DEFS(7)
-GTSAM_MAKE_MATRIX_DEFS(8);
+GTSAM_MAKE_MATRIX_DEFS(8)
-GTSAM_MAKE_MATRIX_DEFS(9);
+GTSAM_MAKE_MATRIX_DEFS(9)
 
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;

gtsam/base/Testable.h

@@ -173,4 +173,4 @@ namespace gtsam {
  * @deprecated please use BOOST_CONCEPT_ASSERT and
  */
 #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
-#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable<T> _gtsam_Testable_##T;
+#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;

gtsam/base/TestableAssertions.h

@@ -85,7 +85,7 @@ bool assert_equal(const V& expected, const boost::optional<const V&>& actual, do
  * \deprecated: use container equals instead
  */
 template<class V>
-bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
+bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
   bool match = true;
   if (expected.size() != actual.size())
     match = false;

gtsam/base/Vector.h

@@ -48,18 +48,18 @@ static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zer
 // Create handy typedefs and constants for vectors with N>3
 // VectorN and Z_Nx1, for N=1..9
 #define GTSAM_MAKE_VECTOR_DEFS(N)                \
-  typedef Eigen::Matrix<double, N, 1> Vector##N; \
+  using Vector##N = Eigen::Matrix<double, N, 1>; \
   static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
 
-GTSAM_MAKE_VECTOR_DEFS(4);
+GTSAM_MAKE_VECTOR_DEFS(4)
-GTSAM_MAKE_VECTOR_DEFS(5);
+GTSAM_MAKE_VECTOR_DEFS(5)
-GTSAM_MAKE_VECTOR_DEFS(6);
+GTSAM_MAKE_VECTOR_DEFS(6)
-GTSAM_MAKE_VECTOR_DEFS(7);
+GTSAM_MAKE_VECTOR_DEFS(7)
-GTSAM_MAKE_VECTOR_DEFS(8);
+GTSAM_MAKE_VECTOR_DEFS(8)
-GTSAM_MAKE_VECTOR_DEFS(9);
+GTSAM_MAKE_VECTOR_DEFS(9)
-GTSAM_MAKE_VECTOR_DEFS(10);
+GTSAM_MAKE_VECTOR_DEFS(10)
-GTSAM_MAKE_VECTOR_DEFS(11);
+GTSAM_MAKE_VECTOR_DEFS(11)
-GTSAM_MAKE_VECTOR_DEFS(12);
+GTSAM_MAKE_VECTOR_DEFS(12)
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
@@ -207,14 +207,14 @@ inline double inner_prod(const V1 &a, const V2& b) {
  * BLAS Level 1 scal: x <- alpha*x
  * \deprecated: use operators instead
  */
-inline void scal(double alpha, Vector& x) { x *= alpha; }
+inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; }
 
 /**
  * BLAS Level 1 axpy: y <- alpha*x + y
  * \deprecated: use operators instead
  */
 template<class V1, class V2>
-inline void axpy(double alpha, const V1& x, V2& y) {
+inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) {
   assert (y.size()==x.size());
   y += alpha * x;
 }

gtsam/base/chartTesting.h

@@ -32,7 +32,7 @@ void testDefaultChart(TestResult& result_,
                       const std::string& name_,
                       const T& value) {
 
-  GTSAM_CONCEPT_TESTABLE_TYPE(T);
+  GTSAM_CONCEPT_TESTABLE_TYPE(T)
 
   typedef typename gtsam::DefaultChart<T> Chart;
   typedef typename Chart::vector Vector;

gtsam/base/serialization.h

@@ -19,8 +19,9 @@
 
 #pragma once
 
-#include <sstream>
+#include <Eigen/Core>
 #include <fstream>
+#include <sstream>
 #include <string>
 
 // includes for standard serialization types
@@ -40,6 +41,17 @@
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/serialization/export.hpp>
 
+// Workaround a bug in GCC >= 7 and C++17
+// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
+#ifdef __GNUC__
+#if __GNUC__ >= 7 && __cplusplus >= 201703L
+namespace boost { namespace serialization { struct U; } }
+namespace Eigen { namespace internal {
+template<> struct traits<boost::serialization::U> {enum {Flags=0};};
+} }
+#endif
+#endif
+
 namespace gtsam {
 
 /** @name Standard serialization

gtsam/base/tests/testVector.cpp

@@ -220,8 +220,8 @@ TEST(Vector, axpy )
   Vector x = Vector3(10., 20., 30.);
   Vector y0 = Vector3(2.0, 5.0, 6.0);
   Vector y1 = y0, y2 = y0;
-  axpy(0.1,x,y1);
+  y1 += 0.1 * x;
-  axpy(0.1,x,y2.head(3));
+  y2.head(3) += 0.1 * x;
   Vector expected = Vector3(3.0, 7.0, 9.0);
   EXPECT(assert_equal(expected,y1));
   EXPECT(assert_equal(expected,Vector(y2)));

gtsam/base/treeTraversal-inst.h

@@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
   // Typedefs
   typedef typename FOREST::Node Node;
 
-  tbb::task::spawn_root_and_wait(
   internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
-          visitorPost, problemSizeThreshold));
+      visitorPost, problemSizeThreshold);
 #else
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 #endif

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -22,7 +22,7 @@
 #include <boost/make_shared.hpp>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/task.h>               // tbb::task, tbb::task_list
+#include <tbb/task_group.h>         // tbb::task_group
 #include <tbb/scalable_allocator.h> // tbb::scalable_allocator
 
 namespace gtsam {
@@ -34,7 +34,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class PreOrderTask : public tbb::task
+      class PreOrderTask
       {
       public:
         const boost::shared_ptr<NODE>& treeNode;
@@ -42,28 +42,30 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
+        tbb::task_group& tg;
         bool makeNewTasks;
 
-        bool isPostOrderPhase;
+        // Keep track of order phase across multiple calls to the same functor
+        mutable bool isPostOrderPhase;
 
         PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
                      VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
-                     bool makeNewTasks = true)
+                     tbb::task_group& tg, bool makeNewTasks = true)
             : treeNode(treeNode),
               myData(myData),
               visitorPre(visitorPre),
               visitorPost(visitorPost),
               problemSizeThreshold(problemSizeThreshold),
+              tg(tg),
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        tbb::task* execute() override
+        void operator()() const
         {
           if(isPostOrderPhase)
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
-            return nullptr;
           }
           else
           {
@@ -71,14 +73,10 @@ namespace gtsam {
             {
               if(!treeNode->children.empty())
               {
-                // Allocate post-order task as a continuation
-                isPostOrderPhase = true;
-                recycle_as_continuation();
-
                 bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
 
-                tbb::task* firstChild = 0;
+                // If we have child tasks, start subtasks and wait for them to complete
-                tbb::task_list childTasks;
+                tbb::task_group ctg;
                 for(const boost::shared_ptr<NODE>& child: treeNode->children)
                 {
                   // Process child in a subtask.  Important:  Run visitorPre before calling
@@ -86,37 +84,30 @@ namespace gtsam {
                   // allocated an extra child, this causes a TBB error.
                   boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
                       tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
-                  tbb::task* childTask =
+                  ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
-                      new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost,
+                      problemSizeThreshold, ctg, overThreshold));
-                                                          problemSizeThreshold, overThreshold);
-                  if (firstChild)
-                    childTasks.push_back(*childTask);
-                  else
-                    firstChild = childTask;
                 }
+                ctg.wait();
 
-                // If we have child tasks, start subtasks and wait for them to complete
+                // Allocate post-order task as a continuation
-                set_ref_count((int)treeNode->children.size());
+                isPostOrderPhase = true;
-                spawn(childTasks);
+                tg.run(*this);
-                return firstChild;
               }
               else
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
-                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
-              return nullptr;
             }
           }
         }
 
-        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
+        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) const
         {
           for(const boost::shared_ptr<NODE>& child: node->children)
           {
@@ -131,7 +122,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class RootTask : public tbb::task
+      class RootTask
       {
       public:
         const ROOTS& roots;
@@ -139,37 +130,30 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
+        tbb::task_group& tg;
         RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-          int problemSizeThreshold) :
+          int problemSizeThreshold, tbb::task_group& tg) :
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-          problemSizeThreshold(problemSizeThreshold) {}
+          problemSizeThreshold(problemSizeThreshold), tg(tg) {}
 
-        tbb::task* execute() override
+        void operator()() const
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
-          tbb::task_list tasks;
           for(const boost::shared_ptr<NODE>& root: roots)
           {
             boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
-            tasks.push_back(*new(allocate_child())
+            tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
-              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
           }
-          // Set TBB ref count
-          set_ref_count(1 + (int) roots.size());
-          // Spawn tasks
-          spawn_and_wait_for_all(tasks);
-          // Return nullptr
-          return nullptr;
         }
       };
 
       template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
+      void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
-        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
       {
           typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
-          return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
+          tbb::task_group tg;
+          tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
       }
 
     }

gtsam/base/types.h

@@ -34,6 +34,14 @@
 #include <tbb/scalable_allocator.h>
 #endif
 
+#if defined(__GNUC__) || defined(__clang__)
+#define GTSAM_DEPRECATED __attribute__((deprecated))
+#elif defined(_MSC_VER)
+#define GTSAM_DEPRECATED __declspec(deprecated)
+#else
+#define GTSAM_DEPRECATED
+#endif
+
 #ifdef GTSAM_USE_EIGEN_MKL_OPENMP
 #include <omp.h>
 #endif

gtsam/config.h.in

@@ -80,3 +80,6 @@
 
 // Whether to use the system installed Metis instead of the provided one
 #cmakedefine GTSAM_USE_SYSTEM_METIS
+
+// Toggle switch for BetweenFactor jacobian computation
+#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR

gtsam/discrete/DiscreteBayesNet.cpp

@@ -54,20 +54,20 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const {
+  DiscreteFactor::Values DiscreteBayesNet::optimize() const {
     // solve each node in turn in topological sort order (parents first)
-    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
+    DiscreteFactor::Values result;
     for (auto conditional: boost::adaptors::reverse(*this))
-      conditional->solveInPlace(*result);
+      conditional->solveInPlace(&result);
     return result;
   }
 
   /* ************************************************************************* */
-  DiscreteFactor::sharedValues DiscreteBayesNet::sample() const {
+  DiscreteFactor::Values DiscreteBayesNet::sample() const {
     // sample each node in turn in topological sort order (parents first)
-    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
+    DiscreteFactor::Values result;
     for (auto conditional: boost::adaptors::reverse(*this))
-      conditional->sampleInPlace(*result);
+      conditional->sampleInPlace(&result);
     return result;
   }
 

gtsam/discrete/DiscreteBayesNet.h

@@ -83,10 +83,10 @@ namespace gtsam {
     /**
     * Solve the DiscreteBayesNet by back-substitution
     */
-    DiscreteFactor::sharedValues optimize() const;
+    DiscreteFactor::Values optimize() const;
 
     /** Do ancestral sampling */
-    DiscreteFactor::sharedValues sample() const;
+    DiscreteFactor::Values sample() const;
 
     ///@}
 

gtsam/discrete/DiscreteConditional.cpp

@@ -35,7 +35,7 @@ using namespace std;
 namespace gtsam {
 
 // Instantiate base class
-template class Conditional<DecisionTreeFactor, DiscreteConditional> ;
+template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional> ;
 
 /* ******************************************************************************** */
 DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
@@ -117,9 +117,9 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
 }
 
 /* ******************************************************************************** */
-void DiscreteConditional::solveInPlace(Values& values) const {
+void DiscreteConditional::solveInPlace(Values* values) const {
   // TODO: Abhijit asks: is this really the fastest way? He thinks it is.
-  ADT pFS = choose(values); // P(F|S=parentsValues)
+  ADT pFS = choose(*values); // P(F|S=parentsValues)
 
   // Initialize
   Values mpe;
@@ -145,16 +145,16 @@ void DiscreteConditional::solveInPlace(Values& values) const {
 
   //set values (inPlace) to mpe
   for(Key j: frontals()) {
-    values[j] = mpe[j];
+    (*values)[j] = mpe[j];
   }
 }
 
 /* ******************************************************************************** */
-void DiscreteConditional::sampleInPlace(Values& values) const {
+void DiscreteConditional::sampleInPlace(Values* values) const {
   assert(nrFrontals() == 1);
   Key j = (firstFrontalKey());
-  size_t sampled = sample(values); // Sample variable
+  size_t sampled = sample(*values); // Sample variable given parents
-  values[j] = sampled; // store result in partial solution
+  (*values)[j] = sampled; // store result in partial solution
 }
 
 /* ******************************************************************************** */

gtsam/discrete/DiscreteConditional.h

@@ -45,7 +45,6 @@ public:
   /** A map from keys to values..
    * TODO: Again, do we need this??? */
   typedef Assignment<Key> Values;
-  typedef boost::shared_ptr<Values> sharedValues;
 
   /// @name Standard Constructors
   /// @{
@@ -133,10 +132,10 @@ public:
   /// @{
 
   /// solve a conditional, in place
-  void solveInPlace(Values& parentsValues) const;
+  void solveInPlace(Values* parentsValues) const;
 
   /// sample in place, stores result in partial solution
-  void sampleInPlace(Values& parentsValues) const;
+  void sampleInPlace(Values* parentsValues) const;
 
   /// @}
 

gtsam/discrete/DiscreteFactor.h

@@ -51,7 +51,6 @@ public:
    * the new class DiscreteValue, as the varible's type (domain)
    */
   typedef Assignment<Key> Values;
-  typedef boost::shared_ptr<Values> sharedValues;
 
 public:
 

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -94,7 +94,7 @@ namespace gtsam {
 //  }
 
   /* ************************************************************************* */
-  DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const
+  DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const
   {
     gttic(DiscreteFactorGraph_optimize);
     return BaseEliminateable::eliminateSequential()->optimize();

gtsam/discrete/DiscreteFactorGraph.h

@@ -74,7 +74,6 @@ public:
   /** A map from keys to values */
   typedef KeyVector Indices;
   typedef Assignment<Key> Values;
-  typedef boost::shared_ptr<Values> sharedValues;
 
   /** Default constructor */
   DiscreteFactorGraph() {}
@@ -101,25 +100,27 @@ public:
 
   /// @}
 
+  // Add single key decision-tree factor.
   template <class SOURCE>
   void add(const DiscreteKey& j, SOURCE table) {
     DiscreteKeys keys;
     keys.push_back(j);
-    push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
+    emplace_shared<DecisionTreeFactor>(keys, table);
   }
 
+  // Add binary key decision-tree factor.
   template <class SOURCE>
   void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) {
     DiscreteKeys keys;
     keys.push_back(j1);
     keys.push_back(j2);
-    push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
+    emplace_shared<DecisionTreeFactor>(keys, table);
   }
 
-  /** add shared discreteFactor immediately from arguments */
+  // Add shared discreteFactor immediately from arguments.
   template <class SOURCE>
   void add(const DiscreteKeys& keys, SOURCE table) {
-    push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
+    emplace_shared<DecisionTreeFactor>(keys, table);
   }
 
   /** Return the set of variables involved in the factors (set union) */
@@ -140,7 +141,7 @@ public:
    *  the dense elimination function specified in \c function,
    *  followed by back-substitution resulting from elimination.  Is equivalent
    *  to calling graph.eliminateSequential()->optimize(). */
-  DiscreteFactor::sharedValues optimize() const;
+  Values optimize() const;
 
 
 //  /** Permute the variables in the factors */

gtsam/discrete/tests/testDiscreteBayesNet.cpp

@@ -104,12 +104,12 @@ TEST(DiscreteBayesNet, Asia) {
   EXPECT(assert_equal(expected2, *chordal->back()));
 
   // solve
-  DiscreteFactor::sharedValues actualMPE = chordal->optimize();
+  auto actualMPE = chordal->optimize();
   DiscreteFactor::Values expectedMPE;
   insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)(
       Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)(
       LungCancer.first, 0)(Bronchitis.first, 0);
-  EXPECT(assert_equal(expectedMPE, *actualMPE));
+  EXPECT(assert_equal(expectedMPE, actualMPE));
 
   // add evidence, we were in Asia and we have dyspnea
   fg.add(Asia, "0 1");
@@ -117,12 +117,12 @@ TEST(DiscreteBayesNet, Asia) {
 
   // solve again, now with evidence
   DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
-  DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize();
+  auto actualMPE2 = chordal2->optimize();
   DiscreteFactor::Values expectedMPE2;
   insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)(
       Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)(
       LungCancer.first, 0)(Bronchitis.first, 1);
-  EXPECT(assert_equal(expectedMPE2, *actualMPE2));
+  EXPECT(assert_equal(expectedMPE2, actualMPE2));
 
   // now sample from it
   DiscreteFactor::Values expectedSample;
@@ -130,8 +130,8 @@ TEST(DiscreteBayesNet, Asia) {
   insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
       Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
       LungCancer.first, 1)(Bronchitis.first, 0);
-  DiscreteFactor::sharedValues actualSample = chordal2->sample();
+  auto actualSample = chordal2->sample();
-  EXPECT(assert_equal(expectedSample, *actualSample));
+  EXPECT(assert_equal(expectedSample, actualSample));
 }
 
 /* ************************************************************************* */

gtsam/discrete/tests/testDiscreteFactorGraph.cpp

@@ -169,8 +169,8 @@ TEST( DiscreteFactorGraph, test)
   // Test optimization
   DiscreteFactor::Values expectedValues;
   insert(expectedValues)(0, 0)(1, 0)(2, 0);
-  DiscreteFactor::sharedValues actualValues = graph.optimize();
+  auto actualValues = graph.optimize();
-  EXPECT(assert_equal(expectedValues, *actualValues));
+  EXPECT(assert_equal(expectedValues, actualValues));
 }
 
 /* ************************************************************************* */
@@ -186,11 +186,11 @@ TEST( DiscreteFactorGraph, testMPE)
   //  graph.product().print();
   //  DiscreteSequentialSolver(graph).eliminate()->print();
 
-  DiscreteFactor::sharedValues actualMPE = graph.optimize();
+  auto actualMPE = graph.optimize();
 
   DiscreteFactor::Values expectedMPE;
   insert(expectedMPE)(0, 0)(1, 1)(2, 1);
-  EXPECT(assert_equal(expectedMPE, *actualMPE));
+  EXPECT(assert_equal(expectedMPE, actualMPE));
 }
 
 /* ************************************************************************* */
@@ -216,8 +216,8 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
 
   // Use the solver machinery.
   DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues actualMPE = chordal->optimize();
+  auto actualMPE = chordal->optimize();
-  EXPECT(assert_equal(expectedMPE, *actualMPE));
+  EXPECT(assert_equal(expectedMPE, actualMPE));
 //  DiscreteConditional::shared_ptr root = chordal->back();
 //  EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9);
 
@@ -244,8 +244,8 @@ ETree::shared_ptr eTree = ETree::Create(graph, structure);
 // eliminate normally and check solution
 DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete);
 //  bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<");
-DiscreteFactor::sharedValues actualMPE = optimize(*bayesNet);
+auto actualMPE = optimize(*bayesNet);
-EXPECT(assert_equal(expectedMPE, *actualMPE));
+EXPECT(assert_equal(expectedMPE, actualMPE));
 
 // Approximate and check solution
 //  DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate();

gtsam/geometry/Cal3Fisheye.cpp

@@ -46,9 +46,9 @@ double Cal3Fisheye::Scaling(double r) {
 /* ************************************************************************* */
 Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
                                 OptionalJacobian<2, 2> H2) const {
-  const double xi = p.x(), yi = p.y();
+  const double xi = p.x(), yi = p.y(), zi = 1;
   const double r2 = xi * xi + yi * yi, r = sqrt(r2);
-  const double t = atan(r);
+  const double t = atan2(r, zi);
   const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
   Vector5 K, T;
   K << 1, k1_, k2_, k3_, k4_;
@@ -76,29 +76,33 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 
   // Derivative for points in intrinsic coords (2 by 2)
   if (H2) {
+    if (r2==0) {
+      *H2 = DK;
+    } else {
       const double dtd_dt =
           1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
-    const double dt_dr = 1 / (1 + r2);
+      const double R2 = r2 + zi*zi;
+      const double dt_dr = zi / R2;
       const double rinv = 1 / r;
       const double dr_dxi = xi * rinv;
       const double dr_dyi = yi * rinv;
-    const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
+      const double dtd_dr = dtd_dt * dt_dr;
-    const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
   
-    const double td = t * K.dot(T);
+      const double c2 = dr_dxi * dr_dxi;
-    const double rrinv = 1 / r2;
+      const double s2 = dr_dyi * dr_dyi;
-    const double dxd_dxi =
+      const double cs = dr_dxi * dr_dyi;
-        dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
+
-    const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
+      const double dxd_dxi = dtd_dr * c2 + s * (1 - c2);
-    const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
+      const double dxd_dyi = (dtd_dr - s) * cs;
-    const double dyd_dyi =
+      const double dyd_dxi = dxd_dyi;
-        dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
+      const double dyd_dyi = dtd_dr * s2 + s * (1 - s2);
 
       Matrix2 DR;
       DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
 
       *H2 = DK * DR;
     }
+  }
 
   return uv;
 }

gtsam/geometry/PinholeCamera.h

@@ -312,6 +312,16 @@ public:
     return range(camera.pose(), Dcamera, Dother);
   }
 
+  /// for Linear Triangulation
+  Matrix34 cameraProjectionMatrix() const {
+    return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
+  }
+
 private:
 
   /** Serialization function */

gtsam/geometry/PinholePose.h

@@ -35,7 +35,7 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
 
 private:
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
 
   // Get dimensions of calibration type at compile time
   static const int DimK = FixedDimension<CALIBRATION>::value;
@@ -121,6 +121,13 @@ public:
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
+  /// project a 3D point from world coordinates into the image
+  Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+    return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
+  }
+
   /// project a point at infinity from world coordinates into the image
   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none,
@@ -159,7 +166,6 @@ public:
     return result;
   }
 
-
   /// backproject a 2-dimensional point to a 3-dimensional point at infinity
   Unit3 backprojectPointAtInfinity(const Point2& p) const {
     const Point2 pn = calibration().calibrate(p);
@@ -410,6 +416,16 @@ public:
     return PinholePose(); // assumes that the default constructor is valid
   }
 
+  /// for Linear Triangulation
+  Matrix34 cameraProjectionMatrix() const {
+    Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
+    return K_->K() * P;
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
   /// @}
 
 private:

gtsam/geometry/Pose2.cpp

@@ -28,7 +28,7 @@ using namespace std;
 namespace gtsam {
 
 /** instantiate concept checks */
-GTSAM_CONCEPT_POSE_INST(Pose2);
+GTSAM_CONCEPT_POSE_INST(Pose2)
 
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 

gtsam/geometry/Pose3.cpp

@@ -30,7 +30,7 @@ using std::vector;
 using Point3Pairs = vector<Point3Pair>;
 
 /** instantiate concept checks */
-GTSAM_CONCEPT_POSE_INST(Pose3);
+GTSAM_CONCEPT_POSE_INST(Pose3)
 
 /* ************************************************************************* */
 Pose3::Pose3(const Pose2& pose2) :
@@ -63,6 +63,46 @@ Matrix6 Pose3::AdjointMap() const {
   return adj;
 }
 
+/* ************************************************************************* */
+// Calculate AdjointMap applied to xi_b, with Jacobians
+Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
+                       OptionalJacobian<6, 6> H_xib) const {
+  const Matrix6 Ad = AdjointMap();
+
+  // Jacobians
+  // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b
+  // D2 Ad_T(xi_b) = Ad_T
+  // See docs/math.pdf for more details.
+  // In D1 calculation, we could be more efficient by writing it out, but do not
+  // for readability
+  if (H_pose) *H_pose = -Ad * adjointMap(xi_b);
+  if (H_xib) *H_xib = Ad;
+
+  return Ad * xi_b;
+}
+
+/* ************************************************************************* */
+/// The dual version of Adjoint
+Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
+                                OptionalJacobian<6, 6> H_x) const {
+  const Matrix6 Ad = AdjointMap();
+  const Vector6 AdTx = Ad.transpose() * x;
+
+  // Jacobians
+  // See docs/math.pdf for more details.
+  if (H_pose) {
+    const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
+               v_T_hat = skewSymmetric(AdTx.tail<3>());
+    *H_pose << w_T_hat, v_T_hat,  //
+        /*  */ v_T_hat, Z_3x3;
+  }
+  if (H_x) {
+    *H_x = Ad.transpose();
+  }
+
+  return AdTx;
+}
+
 /* ************************************************************************* */
 Matrix6 Pose3::adjointMap(const Vector6& xi) {
   Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
@@ -75,7 +115,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
 
 /* ************************************************************************* */
 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6, 6> Hxi) {
+    OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
   if (Hxi) {
     Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
@@ -86,12 +126,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
       Hxi->col(i) = Gi * y;
     }
   }
-  return adjointMap(xi) * y;
+  const Matrix6& ad_xi = adjointMap(xi);
+  if (H_y) *H_y = ad_xi;
+  return ad_xi * y;
 }
 
 /* ************************************************************************* */
 Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6, 6> Hxi) {
+    OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
   if (Hxi) {
     Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
@@ -102,7 +144,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
       Hxi->col(i) = GTi * y;
     }
   }
-  return adjointMap(xi).transpose() * y;
+  const Matrix6& adT_xi = adjointMap(xi).transpose();
+  if (H_y) *H_y = adT_xi;
+  return adT_xi * y;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Pose3.h

@@ -145,15 +145,22 @@ public:
    * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
    * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
    */
-  Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
+  Matrix6 AdjointMap() const;
 
   /**
-   * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
+   * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
+   * body-fixed velocity, transforming it to the spatial frame
    * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
+   * Note that H_xib = AdjointMap()
    */
-  Vector6 Adjoint(const Vector6& xi_b) const {
+  Vector6 Adjoint(const Vector6& xi_b,
-    return AdjointMap() * xi_b;
+                  OptionalJacobian<6, 6> H_this = boost::none,
-  } /// FIXME Not tested - marked as incorrect
+                  OptionalJacobian<6, 6> H_xib = boost::none) const;
+  
+  /// The dual version of Adjoint
+  Vector6 AdjointTranspose(const Vector6& x,
+                           OptionalJacobian<6, 6> H_this = boost::none,
+                           OptionalJacobian<6, 6> H_x = boost::none) const;
 
   /**
    * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
@@ -176,7 +183,8 @@ public:
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
    */
   static Vector6 adjoint(const Vector6& xi, const Vector6& y,
-      OptionalJacobian<6, 6> Hxi = boost::none);
+                         OptionalJacobian<6, 6> Hxi = boost::none,
+                         OptionalJacobian<6, 6> H_y = boost::none);
 
   // temporary fix for wrappers until case issue is resolved
   static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@@ -186,7 +194,8 @@ public:
    * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
    */
   static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
-      OptionalJacobian<6, 6> Hxi = boost::none);
+                                  OptionalJacobian<6, 6> Hxi = boost::none,
+                                  OptionalJacobian<6, 6> H_y = boost::none);
 
   /// Derivative of Expmap
   static Matrix6 ExpmapDerivative(const Vector6& xi);

gtsam/geometry/Quaternion.h

@@ -117,6 +117,7 @@ struct traits<QUATERNION_TYPE> {
       omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
     } else {
       // Normal, away from zero case
+      if (qw > 0) {
         _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
         // Important:  convert to [-pi,pi] to keep error continuous
         if (angle > M_PI)
@@ -124,6 +125,15 @@ struct traits<QUATERNION_TYPE> {
         else if (angle < -M_PI)
           angle += twoPi;
         omega = (angle / s) * q.vec();
+      } else {
+        // Make sure that we are using a canonical quaternion with w > 0
+        _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
+        if (angle > M_PI)
+          angle -= twoPi;
+        else if (angle < -M_PI)
+          angle += twoPi;
+        omega = (angle / s) * -q.vec();
+      }
     }
 
     if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());

gtsam/geometry/SO3.cpp

@@ -261,25 +261,59 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (tr + 1.0 < 1e-10) {
+  if (tr + 1.0 < 1e-3) {
-    if (std::abs(R33 + 1.0) > 1e-5)
+    if (R33 > R22 && R33 > R11) {
-      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+      // R33 is the largest diagonal, a=3, b=1, c=2
-    else if (std::abs(R22 + 1.0) > 1e-5)
+      const double W = R21 - R12;
-      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
+      const double Q1 = 2.0 + 2.0 * R33;
-    else
+      const double Q2 = R31 + R13;
-      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
+      const double Q3 = R23 + R32;
-      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
+    } else if (R22 > R11) {
+      // R22 is the largest diagonal, a=2, b=3, c=1
+      const double W = R13 - R31;
+      const double Q1 = 2.0 + 2.0 * R22;
+      const double Q2 = R23 + R32;
+      const double Q3 = R12 + R21;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
+    } else {
+      // R11 is the largest diagonal, a=1, b=2, c=3
+      const double W = R32 - R23;
+      const double Q1 = 2.0 + 2.0 * R11;
+      const double Q2 = R12 + R21;
+      const double Q3 = R31 + R13;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
+    }
   } else {
     double magnitude;
-    const double tr_3 = tr - 3.0;  // always negative
+    const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
-    if (tr_3 < -1e-7) {
+    if (tr_3 < -1e-6) {
+      // this is the normal case -1 < trace < 3
       double theta = acos((tr - 1.0) / 2.0);
       magnitude = theta / (2.0 * sin(theta));
     } else {
       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
       // see https://github.com/borglab/gtsam/issues/746 for details
-      magnitude = 0.5 - tr_3 / 12.0;
+      magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
     }
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }

gtsam/geometry/Similarity3.cpp

@@ -40,7 +40,7 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
 }
 
 /// Form inner products x and y and calculate scale.
-static const double calculateScale(const Point3Pairs &d_abPointPairs,
+static double calculateScale(const Point3Pairs &d_abPointPairs,
                              const Rot3 &aRb) {
   double x = 0, y = 0;
   Point3 da, db;

gtsam/geometry/SphericalCamera.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SphericalCamera.h
+ * @brief Calibrated camera with spherical projection
+ * @date Aug 26, 2021
+ * @author Luca Carlone
+ */
+
+#include <gtsam/geometry/SphericalCamera.h>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+bool SphericalCamera::equals(const SphericalCamera& camera, double tol) const {
+  return pose_.equals(camera.pose(), tol);
+}
+
+/* ************************************************************************* */
+void SphericalCamera::print(const string& s) const { pose_.print(s + ".pose"); }
+
+/* ************************************************************************* */
+pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
+  const Point3 pc = pose().transformTo(pw);
+  Unit3 pu = Unit3::FromPoint3(pc);
+  return make_pair(pu, pc.norm() > 1e-8);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
+                                OptionalJacobian<2, 3> Dpoint) const {
+  Matrix36 Dtf_pose;
+  Matrix3 Dtf_point;  // calculated by transformTo if needed
+  const Point3 pc =
+      pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
+
+  if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera");
+
+  Matrix23 Dunit;  // calculated by FromPoint3 if needed
+  Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
+
+  if (Dpose) *Dpose = Dunit * Dtf_pose;     // 2x3 * 3x6 = 2x6
+  if (Dpoint) *Dpoint = Dunit * Dtf_point;  // 2x3 * 3x3 = 2x3
+  return pu;
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
+                                OptionalJacobian<2, 2> Dpoint) const {
+  Matrix23 Dtf_rot;
+  Matrix2 Dtf_point;  // calculated by transformTo if needed
+  const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0,
+                                              Dpoint ? &Dtf_point : 0);
+
+  if (Dpose)
+    *Dpose << Dtf_rot, Matrix::Zero(2, 3);  // 2x6 (translation part is zero)
+  if (Dpoint) *Dpoint = Dtf_point;          // 2x2
+  return pu;
+}
+
+/* ************************************************************************* */
+Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
+  return pose().transformFrom(depth * pu);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
+  return pose().rotation().rotate(p);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project(const Point3& point,
+                               OptionalJacobian<2, 6> Dcamera,
+                               OptionalJacobian<2, 3> Dpoint) const {
+  return project2(point, Dcamera, Dpoint);
+}
+
+/* ************************************************************************* */
+Vector2 SphericalCamera::reprojectionError(
+    const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 3> Dpoint) const {
+  // project point
+  if (Dpose || Dpoint) {
+    Matrix26 H_project_pose;
+    Matrix23 H_project_point;
+    Matrix22 H_error;
+    Unit3 projected = project2(point, H_project_pose, H_project_point);
+    Vector2 error = measured.errorVector(projected, boost::none, H_error);
+    if (Dpose) *Dpose = H_error * H_project_pose;
+    if (Dpoint) *Dpoint = H_error * H_project_point;
+    return error;
+  } else {
+    return measured.errorVector(project2(point, Dpose, Dpoint));
+  }
+}
+
+/* ************************************************************************* */
+}  // namespace gtsam

gtsam/geometry/SphericalCamera.h

@@ -0,0 +1,225 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SphericalCamera.h
+ * @brief Calibrated camera with spherical projection
+ * @date Aug 26, 2021
+ * @author Luca Carlone
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/ThreadsafeException.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/dllexport.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Unit3.h>
+
+#include <boost/serialization/nvp.hpp>
+
+namespace gtsam {
+
+/**
+ * Empty calibration. Only needed to play well with other cameras
+ * (e.g., when templating functions wrt cameras), since other cameras
+ * have constuctors in the form ‘camera(pose,calibration)’
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT EmptyCal {
+ public:
+  enum { dimension = 0 };
+  EmptyCal() {}
+  virtual ~EmptyCal() = default;
+  using shared_ptr = boost::shared_ptr<EmptyCal>;
+  void print(const std::string& s) const {
+    std::cout << "empty calibration: " << s << std::endl;
+  }
+};
+
+/**
+ * A spherical camera class that has a Pose3 and measures bearing vectors.
+ * The camera has an ‘Empty’ calibration and the only 6 dof are the pose
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT SphericalCamera {
+ public:
+  enum { dimension = 6 };
+
+  typedef Unit3 Measurement;
+  typedef std::vector<Unit3> MeasurementVector;
+  typedef EmptyCal CalibrationType;
+
+ private:
+  Pose3 pose_;  ///< 3D pose of camera
+
+ protected:
+  EmptyCal::shared_ptr emptyCal_;
+
+ public:
+  /// @}
+  /// @name Standard Constructors
+  /// @{
+
+  /// Default constructor
+  SphericalCamera()
+      : pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// Constructor with pose
+  explicit SphericalCamera(const Pose3& pose)
+      : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// Constructor with empty intrinsics (needed for smart factors)
+  explicit SphericalCamera(const Pose3& pose,
+                           const boost::shared_ptr<EmptyCal>& cal)
+      : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+  explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
+
+  /// Default destructor
+  virtual ~SphericalCamera() = default;
+
+  /// return shared pointer to calibration
+  const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
+    return emptyCal_;
+  }
+
+  /// return calibration
+  const EmptyCal& calibration() const { return *emptyCal_; }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
+
+  /// print
+  virtual void print(const std::string& s = "SphericalCamera") const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// return pose, constant version
+  const Pose3& pose() const { return pose_; }
+
+  /// get rotation
+  const Rot3& rotation() const { return pose_.rotation(); }
+
+  /// get translation
+  const Point3& translation() const { return pose_.translation(); }
+
+  //  /// return pose, with derivative
+  //  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /// Project a point into the image and check depth
+  std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+                 OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D direction in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
+                 OptionalJacobian<2, 2> Dpoint = boost::none) const;
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  Point3 backproject(const Unit3& p, const double depth) const;
+
+  /// backproject point at infinity
+  Unit3 backprojectPointAtInfinity(const Unit3& p) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
+                OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /** Compute reprojection error for a given 3D point in world coordinates
+   * @param point 3D point in world coordinates
+   * @return the tangent space error between the projection and the measurement
+   */
+  Vector2 reprojectionError(const Point3& point, const Unit3& measured,
+                            OptionalJacobian<2, 6> Dpose = boost::none,
+                            OptionalJacobian<2, 3> Dpoint = boost::none) const;
+  /// @}
+
+  /// move a cameras according to d
+  SphericalCamera retract(const Vector6& d) const {
+    return SphericalCamera(pose().retract(d));
+  }
+
+  /// return canonical coordinate
+  Vector6 localCoordinates(const SphericalCamera& p) const {
+    return pose().localCoordinates(p.pose());
+  }
+
+  /// for Canonical
+  static SphericalCamera identity() {
+    return SphericalCamera(
+        Pose3::identity());  // assumes that the default constructor is valid
+  }
+
+  /// for Linear Triangulation
+  Matrix34 cameraProjectionMatrix() const {
+    return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
+  }
+
+  /// @deprecated
+  size_t dim() const { return 6; }
+
+  /// @deprecated
+  static size_t Dim() { return 6; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(pose_);
+  }
+};
+// end of class SphericalCamera
+
+template <>
+struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
+
+template <>
+struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
+
+}  // namespace gtsam

gtsam/geometry/StereoCamera.h

@@ -170,6 +170,11 @@ public:
       OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
           boost::none) const;
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
+
   /// @}
 
 private:

gtsam/geometry/concepts.h

@@ -72,5 +72,5 @@ private:
 
 /** Pose Concept macros */
 #define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
-#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;
+#define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept<T>;
 

gtsam/geometry/geometry.i

@@ -473,6 +473,9 @@ class Pose3 {
   Vector logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  Vector AdjointTranspose(Vector xi) const;
+  static Matrix adjointMap(Vector xi);
+  static Vector adjoint(Vector xi, Vector y);
   static Matrix adjointMap_(Vector xi);
   static Vector adjoint_(Vector xi, Vector y);
   static Vector adjointTranspose(Vector xi, Vector y);

gtsam/geometry/tests/testPose3.cpp

@@ -145,6 +145,81 @@ TEST(Pose3, Adjoint_full)
   EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
 }
 
+/* ************************************************************************* */
+// Check Adjoint numerical derivatives
+TEST(Pose3, Adjoint_jacobians)
+{
+  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
+
+  // Check evaluation sanity check
+  EQUALITY(static_cast<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
+  EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
+  EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
+
+  // Check jacobians
+  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
+  std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
+      [&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
+
+  T.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T2.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T3.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
+/* ************************************************************************* */
+// Check AdjointTranspose and jacobians
+TEST(Pose3, AdjointTranspose)
+{
+  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
+
+  // Check evaluation
+  EQUALITY(static_cast<Vector>(T.AdjointMap().transpose() * xi),
+           T.AdjointTranspose(xi));
+  EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
+           T2.AdjointTranspose(xi));
+  EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
+           T3.AdjointTranspose(xi));
+
+  // Check jacobians
+  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
+  std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
+      [&](const Pose3& T, const Vector6& xi) {
+        return T.AdjointTranspose(xi);
+      };
+
+  T.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T2.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T3.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
 /* ************************************************************************* */
 // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
 TEST(Pose3, Adjoint_hat)
@@ -837,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
 }
 
 TEST( Pose3, adjoint) {
-  Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);
+  Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished();
+  Vector expected = testDerivAdjoint(screwPose3::xi, v);
 
-  Matrix actualH;
+  Matrix actualH1, actualH2;
-  Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);
+  Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2);
 
-  Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
+  Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
-      testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
+      testDerivAdjoint, screwPose3::xi, v, 1e-5);
+  Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
+      testDerivAdjoint, screwPose3::xi, v, 1e-5);
 
   EXPECT(assert_equal(expected,actual,1e-5));
-  EXPECT(assert_equal(numericalH,actualH,1e-5));
+  EXPECT(assert_equal(numericalH1,actualH1,1e-5));
+  EXPECT(assert_equal(numericalH2,actualH2,1e-5));
 }
 
 /* ************************************************************************* */
@@ -859,14 +938,17 @@ TEST( Pose3, adjointTranspose) {
   Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
   Vector expected = testDerivAdjointTranspose(xi, v);
 
-  Matrix actualH;
+  Matrix actualH1, actualH2;
-  Vector actual = Pose3::adjointTranspose(xi, v, actualH);
+  Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2);
 
-  Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
+  Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
+      testDerivAdjointTranspose, xi, v, 1e-5);
+  Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
       testDerivAdjointTranspose, xi, v, 1e-5);
 
   EXPECT(assert_equal(expected,actual,1e-15));
-  EXPECT(assert_equal(numericalH,actualH,1e-5));
+  EXPECT(assert_equal(numericalH1,actualH1,1e-5));
+  EXPECT(assert_equal(numericalH2,actualH2,1e-5));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual3,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Rot3, AxisAngle2)
+{
+  // constructor from a rotation matrix, as doubles in *row-major* order.
+  Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
+  
+  Unit3 actualAxis;
+  double actualAngle;
+  // convert Rot3 to quaternion using GTSAM
+  std::tie(actualAxis, actualAngle) = R1.axisAngle();
+  
+  double expectedAngle = 3.1396582;
+  CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, Rodrigues)
 {
@@ -187,7 +202,7 @@ TEST(Rot3, log) {
   Rot3 R;
 
 #define CHECK_OMEGA(X, Y, Z)             \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
 
@@ -219,17 +234,17 @@ TEST(Rot3, log) {
   CHECK_OMEGA(0, 0, PI)
 
   // Windows and Linux have flipped sign in quaternion mode
-#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
+//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
   w = (Vector(3) << x * PI, y * PI, z * PI).finished();
   R = Rot3::Rodrigues(w);
   EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
-#else
+//#else
-  CHECK_OMEGA(x * PI, y * PI, z * PI)
+//  CHECK_OMEGA(x * PI, y * PI, z * PI)
-#endif
+//#endif
 
   // Check 360 degree rotations
 #define CHECK_OMEGA_ZERO(X, Y, Z)        \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
 
@@ -252,8 +267,8 @@ TEST(Rot3, log) {
   EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
                       (Vector)Rot3::Logmap(Rlund), 1e-8));
 #else
-    // SO3 does not bound angle resulting in ~180.1 degrees
+  // SO3 will be approximate because of the non-orthogonality
-    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
+  EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184),
                         (Vector)Rot3::Logmap(Rlund), 1e-8));
 #endif
 }

gtsam/geometry/tests/testSphericalCamera.cpp

@@ -0,0 +1,163 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SphericalCamera.h
+ * @brief Calibrated camera with spherical projection
+ * @date Aug 26, 2021
+ * @author Luca Carlone
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/SphericalCamera.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace std::placeholders;
+using namespace std;
+using namespace gtsam;
+
+typedef SphericalCamera Camera;
+
+// static const Cal3_S2 K(625, 625, 0, 0, 0);
+//
+static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()),
+                        Point3(0, 0, 0.5));
+static const Camera camera(pose);
+//
+static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
+static const Camera camera1(pose1);
+
+static const Point3 point1(-0.08, -0.08, 0.0);
+static const Point3 point2(-0.08, 0.08, 0.0);
+static const Point3 point3(0.08, 0.08, 0.0);
+static const Point3 point4(0.08, -0.08, 0.0);
+
+// manually computed in matlab
+static const Unit3 bearing1(-0.156054862928174, 0.156054862928174,
+                            0.975342893301088);
+static const Unit3 bearing2(-0.156054862928174, -0.156054862928174,
+                            0.975342893301088);
+static const Unit3 bearing3(0.156054862928174, -0.156054862928174,
+                            0.975342893301088);
+static const Unit3 bearing4(0.156054862928174, 0.156054862928174,
+                            0.975342893301088);
+
+static double depth = 0.512640224719052;
+/* ************************************************************************* */
+TEST(SphericalCamera, constructor) {
+  EXPECT(assert_equal(pose, camera.pose()));
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, project) {
+  // expected from manual calculation in Matlab
+  EXPECT(assert_equal(camera.project(point1), bearing1));
+  EXPECT(assert_equal(camera.project(point2), bearing2));
+  EXPECT(assert_equal(camera.project(point3), bearing3));
+  EXPECT(assert_equal(camera.project(point4), bearing4));
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, backproject) {
+  EXPECT(assert_equal(camera.backproject(bearing1, depth), point1));
+  EXPECT(assert_equal(camera.backproject(bearing2, depth), point2));
+  EXPECT(assert_equal(camera.backproject(bearing3, depth), point3));
+  EXPECT(assert_equal(camera.backproject(bearing4, depth), point4));
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, backproject2) {
+  Point3 origin(0, 0, 0);
+  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.);  // a camera1 looking down
+  Camera camera(Pose3(rot, origin));
+
+  Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.);
+  Point3 expected(0., 1., 0.);
+  pair<Unit3, bool> x = camera.projectSafe(expected);
+
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(Unit3(0, 0, 1), x.first));
+  EXPECT(x.second);
+}
+
+/* ************************************************************************* */
+static Unit3 project3(const Pose3& pose, const Point3& point) {
+  return Camera(pose).project(point);
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, Dproject) {
+  Matrix Dpose, Dpoint;
+  Unit3 result = camera.project(point1, Dpose, Dpoint);
+  Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
+  Matrix numerical_point = numericalDerivative22(project3, pose, point1);
+  EXPECT(assert_equal(bearing1, result));
+  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
+  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
+                                  const Unit3& measured) {
+  return Camera(pose).reprojectionError(point, measured);
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, reprojectionError) {
+  Matrix Dpose, Dpoint;
+  Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
+  Matrix numerical_pose =
+      numericalDerivative31(reprojectionError2, pose, point1, bearing1);
+  Matrix numerical_point =
+      numericalDerivative32(reprojectionError2, pose, point1, bearing1);
+  EXPECT(assert_equal(Vector2(0.0, 0.0), result));
+  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
+  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(SphericalCamera, reprojectionError_noisy) {
+  Matrix Dpose, Dpoint;
+  Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
+  Vector2 result =
+      camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint);
+  Matrix numerical_pose =
+      numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
+  Matrix numerical_point =
+      numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
+  EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
+  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
+  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST(SphericalCamera, Dproject2) {
+  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Camera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
+  Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
+  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/geometry/tests/testTriangulation.cpp

@@ -10,22 +10,23 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * testTriangulation.cpp
+ * @file testTriangulation.cpp
- *
+ * @brief triangulation utilities
- *  Created on: July 30th, 2013
+ * @date July 30th, 2013
- *      Author: cbeall3
+ * @author Chris Beall (cbeall3)
+ * @author Luca Carlone
  */
 
-#include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/StereoCamera.h>
-#include <gtsam/geometry/CameraSet.h>
-#include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam/slam/StereoFactor.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <CppUnitLite/TestHarness.h>
-
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/SphericalCamera.h>
+#include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/geometry/triangulation.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/StereoFactor.h>
 
 #include <boost/assign.hpp>
 #include <boost/assign/std/vector.hpp>
@@ -58,7 +59,6 @@ Point2 z2 = camera2.project(landmark);
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
 TEST(triangulation, twoPoses) {
-
   vector<Pose3> poses;
   Point2Vector measurements;
 
@@ -70,34 +70,34 @@ TEST( triangulation, twoPoses) {
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(landmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(landmark, *actual2, 1e-7));
 
-  // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  // 3. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
   optimize = false;
   boost::optional<Point3> actual3 =  //
-      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
 
   // 4. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 =  //
-      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
 //******************************************************************************
 // Similar, but now with Bundler calibration
 TEST(triangulation, twoPosesBundler) {
-
   boost::shared_ptr<Cal3Bundler> bundlerCal =  //
       boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
   PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
@@ -117,7 +117,7 @@ TEST( triangulation, twoPosesBundler) {
   double rank_tol = 1e-9;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(landmark, *actual, 1e-7));
 
   // Add some noise and try again
@@ -125,7 +125,7 @@ TEST( triangulation, twoPosesBundler) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
 }
 
@@ -137,16 +137,17 @@ TEST( triangulation, fourPoses) {
   poses += pose1, pose2;
   measurements += z1, z2;
 
-  boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
+  boost::optional<Point3> actual =
-      measurements);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
   EXPECT(assert_equal(landmark, *actual, 1e-2));
 
-  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  // 2. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
   EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
@@ -158,12 +159,12 @@ TEST( triangulation, fourPoses) {
   measurements += z3 + Point2(0.1, -0.1);
 
   boost::optional<Point3> triangulated_3cameras =  //
-      triangulatePoint3(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
-  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
+  boost::optional<Point3> triangulated_3cameras_opt =
-      sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
@@ -176,7 +177,7 @@ TEST( triangulation, fourPoses) {
   poses += pose4;
   measurements += Point2(400, 400);
 
-  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
                   TriangulationCheiralityException);
 #endif
 }
@@ -202,15 +203,16 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   measurements += z1, z2;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3(cameras, measurements);
+      triangulatePoint3<Cal3_S2>(cameras, measurements);
   EXPECT(assert_equal(landmark, *actual, 1e-2));
 
-  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  // 2. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3(cameras, measurements);
+      triangulatePoint3<Cal3_S2>(cameras, measurements);
   EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
@@ -223,12 +225,12 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   measurements += z3 + Point2(0.1, -0.1);
 
   boost::optional<Point3> triangulated_3cameras =  //
-      triangulatePoint3(cameras, measurements);
+      triangulatePoint3<Cal3_S2>(cameras, measurements);
   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
-  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras,
+  boost::optional<Point3> triangulated_3cameras_opt =
-      measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true);
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
@@ -241,7 +243,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
 
   cameras += camera4;
   measurements += Point2(400, 400);
-  CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements),
                   TriangulationCheiralityException);
 #endif
 }
@@ -267,17 +269,22 @@ TEST( triangulation, outliersAndFarLandmarks) {
   measurements += z1, z2;
 
   double landmarkDistanceThreshold = 10;  // landmark is closer than that
-  TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
+  TriangulationParameters params(
+      1.0, false, landmarkDistanceThreshold);  // all default except
+                                               // landmarkDistanceThreshold
   TriangulationResult actual = triangulateSafe(cameras, measurements, params);
   EXPECT(assert_equal(landmark, *actual, 1e-2));
   EXPECT(actual.valid());
 
   landmarkDistanceThreshold = 4;  // landmark is farther than that
-  TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
+  TriangulationParameters params2(
+      1.0, false, landmarkDistanceThreshold);  // all default except
+                                               // landmarkDistanceThreshold
   actual = triangulateSafe(cameras, measurements, params2);
   EXPECT(actual.farPoint());
 
-  // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
+  // 3. Add a slightly rotated third camera above with a wrong measurement
+  // (OUTLIER)
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
   PinholeCamera<Cal3_S2> camera3(pose3, K3);
@@ -288,13 +295,15 @@ TEST( triangulation, outliersAndFarLandmarks) {
 
   landmarkDistanceThreshold = 10;  // landmark is closer than that
   double outlierThreshold = 100;   // loose, the outlier is going to pass
-  TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold);
+  TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,
+                                  outlierThreshold);
   actual = triangulateSafe(cameras, measurements, params3);
   EXPECT(actual.valid());
 
   // now set stricter threshold for outlier rejection
   outlierThreshold = 5;  // tighter, the outlier is not going to pass
-  TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold);
+  TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,
+                                  outlierThreshold);
   actual = triangulateSafe(cameras, measurements, params4);
   EXPECT(actual.outlier());
 }
@@ -313,7 +322,7 @@ TEST( triangulation, twoIdenticalPoses) {
   poses += pose1, pose1;
   measurements += z1, z1;
 
-  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
                   TriangulationUnderconstrainedException);
 }
 
@@ -328,26 +337,24 @@ TEST( triangulation, onePose) {
   poses += Pose3();
   measurements += Point2(0, 0);
 
-  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
                   TriangulationUnderconstrainedException);
 }
 
 //******************************************************************************
 TEST(triangulation, StereotriangulateNonlinear) {
-
+  auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645,
-  auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
+                                                   508.835, 0.0699612);
 
   // two camera poses m1, m2
   Matrix4 m1, m2;
-  m1 << 0.796888717,     0.603404026,   -0.0295271487, 46.6673779,
+  m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
-      0.592783835,    -0.77156583,    0.230856632,   66.2186159,
+      -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
-      0.116517574,   -0.201470143,     -0.9725393, -4.28382528,
+      -0.9725393, -4.28382528, 0, 0, 0, 1;
-      0, 0, 0, 1;
 
-  m2 << -0.955959025,    -0.29288915,   -0.0189328569, 45.7169799,
+  m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, -0.29277519,
-      -0.29277519,    0.947083213,    0.131587097, 65.843136,
+      0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
-     -0.0206094928,   0.131334858,   -0.991123524, -4.3525033,
+      -0.991123524, -4.3525033, 0, 0, 0, 1;
-     0, 0, 0, 1;
 
   typedef CameraSet<StereoCamera> Cameras;
   Cameras cameras;
@@ -358,15 +365,16 @@ TEST( triangulation, StereotriangulateNonlinear ) {
   measurements += StereoPoint2(226.936, 175.212, 424.469);
   measurements += StereoPoint2(339.571, 285.547, 669.973);
 
-  Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929);  // error: 96.5715555191
+  Point3 initial =
+      Point3(46.0536958, 66.4621179, -6.56285929);  // error: 96.5715555191
 
-  Point3 actual = triangulateNonlinear(cameras, measurements, initial);
+  Point3 actual = triangulateNonlinear<StereoCamera>(cameras, measurements, initial);
 
-  Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187
+  Point3 expected(46.0484569, 66.4710686,
+                  -6.55046613);  // error: 0.763510644187
 
   EXPECT(assert_equal(expected, actual, 1e-4));
 
-
   // regular stereo factor comparison - expect very similar result as above
   {
     typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
@@ -378,8 +386,10 @@ TEST( triangulation, StereotriangulateNonlinear ) {
 
     NonlinearFactorGraph graph;
     static SharedNoiseModel unit(noiseModel::Unit::Create(3));
-    graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK);
+    graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x', 1),
-    graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
+                                       Symbol('l', 1), stereoK);
+    graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x', 2),
+                                       Symbol('l', 1), stereoK);
 
     const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
     graph.addPrior(Symbol('x', 1), Pose3(m1), posePrior);
@@ -399,8 +409,10 @@ TEST( triangulation, StereotriangulateNonlinear ) {
     NonlinearFactorGraph graph;
     static SharedNoiseModel unit(noiseModel::Unit::Create(3));
 
-    graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1));
+    graph.emplace_shared<TriangulationFactor<StereoCamera>>(
-    graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1));
+        cameras[0], measurements[0], unit, Symbol('l', 1));
+    graph.emplace_shared<TriangulationFactor<StereoCamera>>(
+        cameras[1], measurements[1], unit, Symbol('l', 1));
 
     LevenbergMarquardtOptimizer optimizer(graph, values);
     Values result = optimizer.optimize();
@@ -419,8 +431,10 @@ TEST( triangulation, StereotriangulateNonlinear ) {
     Expression<Point3> point_(Symbol('l', 1));
     Expression<StereoCamera> camera0_(cameras[0]);
     Expression<StereoCamera> camera1_(cameras[1]);
-    Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
+    Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2,
-    Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);
+                                       point_);
+    Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2,
+                                       point_);
 
     graph.addExpressionFactor(unit, measurements[0], project0_);
     graph.addExpressionFactor(unit, measurements[1], project1_);
@@ -432,6 +446,168 @@ TEST( triangulation, StereotriangulateNonlinear ) {
   }
 }
 
+//******************************************************************************
+// Simple test with a well-behaved two camera situation
+TEST(triangulation, twoPoses_sphericalCamera) {
+  vector<Pose3> poses;
+  std::vector<Unit3> measurements;
+
+  // Project landmark into two cameras and triangulate
+  SphericalCamera cam1(pose1);
+  SphericalCamera cam2(pose2);
+  Unit3 u1 = cam1.project(landmark);
+  Unit3 u2 = cam2.project(landmark);
+
+  poses += pose1, pose2;
+  measurements += u1, u2;
+
+  CameraSet<SphericalCamera> cameras;
+  cameras.push_back(cam1);
+  cameras.push_back(cam2);
+
+  double rank_tol = 1e-9;
+
+  // 1. Test linear triangulation via DLT
+  auto projection_matrices = projectionMatricesFromCameras(cameras);
+  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
+  EXPECT(assert_equal(landmark, point, 1e-7));
+
+  // 2. Test nonlinear triangulation
+  point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
+  EXPECT(assert_equal(landmark, point, 1e-7));
+
+  // 3. Test simple DLT, now within triangulatePoint3
+  bool optimize = false;
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+
+  // 4. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize);
+  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+
+  // 5. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
+  measurements.at(0) =
+      u1.retract(Vector2(0.01, 0.05));  // note: perturbation smaller for Unit3
+  measurements.at(1) = u2.retract(Vector2(-0.02, 0.03));
+  optimize = false;
+  boost::optional<Point3> actual3 =  //
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize);
+  EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3));
+
+  // 6. Now with optimization on
+  optimize = true;
+  boost::optional<Point3> actual4 =  //
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize);
+  EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
+}
+
+//******************************************************************************
+TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
+  vector<Pose3> poses;
+  std::vector<Unit3> measurements;
+
+  // Project landmark into two cameras and triangulate
+  Pose3 poseA = Pose3(
+      Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
+      Point3(0.0, 0.0, 0.0));  // with z pointing along x axis of global frame
+  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
+                      Point3(2.0, 0.0, 0.0));  // 2m in front of poseA
+  Point3 landmarkL(
+      1.0, -1.0,
+      0.0);  // 1m to the right of both cameras, in front of poseA, behind poseB
+  SphericalCamera cam1(poseA);
+  SphericalCamera cam2(poseB);
+  Unit3 u1 = cam1.project(landmarkL);
+  Unit3 u2 = cam2.project(landmarkL);
+
+  EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, 1.0)), u1,
+                      1e-7));  // in front and to the right of PoseA
+  EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
+                      1e-7));  // behind and to the right of PoseB
+
+  poses += pose1, pose2;
+  measurements += u1, u2;
+
+  CameraSet<SphericalCamera> cameras;
+  cameras.push_back(cam1);
+  cameras.push_back(cam2);
+
+  double rank_tol = 1e-9;
+
+  {
+    // 1. Test simple DLT, when 1 point is behind spherical camera
+    bool optimize = false;
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+    CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
+                                                       rank_tol, optimize),
+                    TriangulationCheiralityException);
+#else  // otherwise project should not throw the exception
+    boost::optional<Point3> actual1 =  //
+        triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                           optimize);
+    EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
+#endif
+  }
+  {
+    // 2. test with optimization on, same answer
+    bool optimize = true;
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+    CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
+                                                       rank_tol, optimize),
+                    TriangulationCheiralityException);
+#else  // otherwise project should not throw the exception
+    boost::optional<Point3> actual1 =  //
+        triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                           optimize);
+    EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
+#endif
+  }
+}
+
+//******************************************************************************
+TEST(triangulation, reprojectionError_cameraComparison) {
+  Pose3 poseA = Pose3(
+      Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
+      Point3(0.0, 0.0, 0.0));  // with z pointing along x axis of global frame
+  Point3 landmarkL(5.0, 0.0, 0.0);  // 1m in front of poseA
+  SphericalCamera sphericalCamera(poseA);
+  Unit3 u = sphericalCamera.project(landmarkL);
+
+  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(60, 640, 480));
+  PinholePose<Cal3_S2> pinholeCamera(poseA, sharedK);
+  Vector2 px = pinholeCamera.project(landmarkL);
+
+  // add perturbation and compare error in both cameras
+  Vector2 px_noise(1.0, 2.0);  // px perturbation vertically and horizontally
+  Vector2 measured_px = px + px_noise;
+  Vector2 measured_px_calibrated = sharedK->calibrate(measured_px);
+  Unit3 measured_u =
+      Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0);
+  Unit3 expected_measured_u =
+      Unit3(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy(), 1.0);
+  EXPECT(assert_equal(expected_measured_u, measured_u, 1e-7));
+
+  Vector2 actualErrorPinhole =
+      pinholeCamera.reprojectionError(landmarkL, measured_px);
+  Vector2 expectedErrorPinhole = Vector2(-px_noise[0], -px_noise[1]);
+  EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole,
+                      1e-7));  //- sign due to definition of error
+
+  Vector2 actualErrorSpherical =
+      sphericalCamera.reprojectionError(landmarkL, measured_u);
+  // expectedError: not easy to calculate, since it involves the unit3 basis
+  Vector2 expectedErrorSpherical(-0.00360842, 0.00180419);
+  EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;

gtsam/geometry/triangulation.cpp

@@ -53,11 +53,53 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
-Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
+
+  // number of cameras
+  size_t m = projection_matrices.size();
+
+  // Allocate DLT matrix
+  Matrix A = Matrix::Zero(m * 2, 4);
+
+  for (size_t i = 0; i < m; i++) {
+    size_t row = i * 2;
+    const Matrix34& projection = projection_matrices.at(i);
+    const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
+
+    // build system of equations
+    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
+    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
+  }
+  int rank;
+  double error;
+  Vector v;
+  boost::tie(rank, error, v) = DLT(A, rank_tol);
+
+  if (rank < 3)
+    throw(TriangulationUnderconstrainedException());
+
+  return v;
+}
+
+Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
 
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
+                                        rank_tol);
+  // Create 3D point from homogeneous coordinates
+  return Point3(v.head<3>() / v[3]);
+}
 
+Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
+
+  // contrary to previous triangulateDLT, this is now taking Unit3 inputs
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
+                                         rank_tol);
    // Create 3D point from homogeneous coordinates
    return Point3(v.head<3>() / v[3]);
 }

gtsam/geometry/triangulation.h

@@ -24,6 +24,7 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -59,6 +60,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
+/**
+ * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
+ * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
+ * @param projection_matrices Projection matrices (K*P^-1)
+ * @param measurements Unit3 bearing measurements
+ * @param rank_tol SVD rank tolerance
+ * @return Triangulated point, in homogeneous coordinates
+ */
+GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
+
 /**
  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
  * @param projection_matrices Projection matrices (K*P^-1)
@@ -71,6 +84,14 @@ GTSAM_EXPORT Point3 triangulateDLT(
     const Point2Vector& measurements,
     double rank_tol = 1e-9);
 
+/**
+ * overload of previous function to work with Unit3 (projected to canonical camera)
+ */
+GTSAM_EXPORT Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements,
+    double rank_tol = 1e-9);
+
 /**
  * Create a factor graph with projection factors from poses and one calibration
  * @param poses Camera poses
@@ -180,26 +201,27 @@ Point3 triangulateNonlinear(
   return optimize(graph, values, Symbol('p', 0));
 }
 
-/**
+template<class CAMERA>
- * Create a 3*4 camera projection matrix from calibration and pose.
+std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
- * Functor for partial application on calibration
+projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
- * @param pose The camera pose
+  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
- * @param cal  The calibration
+  for (const CAMERA &camera: cameras) {
- * @return Returns a Matrix34
+    projection_matrices.push_back(camera.cameraProjectionMatrix());
- */
+  }
+  return projection_matrices;
+}
+
+// overload, assuming pinholePose
 template<class CALIBRATION>
-struct CameraProjectionMatrix {
+std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
-  CameraProjectionMatrix(const CALIBRATION& calibration) :
+        const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
-      K_(calibration.K()) {
+  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
+  for (size_t i = 0; i < poses.size(); i++) {
+    PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
+    projection_matrices.push_back(camera.cameraProjectionMatrix());
   }
-  Matrix34 operator()(const Pose3& pose) const {
+  return projection_matrices;
-    return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
 }
-private:
-  const Matrix3 K_;
-public:
-  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
-};
 
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
@@ -224,10 +246,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
+  auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
-  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
-  for(const Pose3& pose: poses)
-    projection_matrices.push_back(createP(pose));
 
   // Triangulate linearly
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@@ -274,11 +293,7 @@ Point3 triangulatePoint3(
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
+  auto projection_matrices = projectionMatricesFromCameras(cameras);
-  for(const CAMERA& camera: cameras)
-    projection_matrices.push_back(
-        CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
-            camera.pose()));
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
 
   // The n refine using non-linear optimization
@@ -474,8 +489,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 #endif
         // Check reprojection error
         if (params.dynamicOutlierRejectionThreshold > 0) {
-          const Point2& zi = measured.at(i);
+          const typename CAMERA::Measurement& zi = measured.at(i);
-          Point2 reprojectionError(camera.project(point) - zi);
+          Point2 reprojectionError = camera.reprojectionError(point, zi);
           maxReprojError = std::max(maxReprojError, reprojectionError.norm());
         }
         i += 1;
@@ -503,6 +518,6 @@ using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
-
+using CameraSetSpherical = CameraSet<SphericalCamera>;
 } // \namespace gtsam
 

gtsam/inference/BayesTree.h

@@ -19,6 +19,8 @@
 
 #pragma once
 
+#include <boost/shared_ptr.hpp>
+
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/FastList.h>
 #include <gtsam/base/ConcurrentMap.h>

gtsam/inference/ClusterTree.h

@@ -110,7 +110,7 @@ class ClusterTree {
   typedef sharedCluster sharedNode;
 
   /** concept check */
-  GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
+  GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
 
  protected:
   FastVector<sharedNode> roots_;

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -36,17 +36,17 @@ namespace gtsam {
       // no Ordering is provided.  When removing optional from VariableIndex, create VariableIndex
       // before creating ordering.
       VariableIndex computedVariableIndex(asDerived());
-      return eliminateSequential(function, computedVariableIndex, orderingType);
+      return eliminateSequential(orderingType, function, computedVariableIndex);
     }
     else {
       // Compute an ordering and call this function again.  We are guaranteed to have a 
       // VariableIndex already here because we computed one if needed in the previous 'if' block.
       if (orderingType == Ordering::METIS) {
         Ordering computedOrdering = Ordering::Metis(asDerived());
-        return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
+        return eliminateSequential(computedOrdering, function, variableIndex);
       } else {
         Ordering computedOrdering = Ordering::Colamd(*variableIndex);
-        return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
+        return eliminateSequential(computedOrdering, function, variableIndex);
       }
     }
   }
@@ -79,28 +79,30 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template <class FACTORGRAPH>
-  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+  boost::shared_ptr<
+      typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
   EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
       OptionalOrderingType orderingType, const Eliminate& function,
-    OptionalVariableIndex variableIndex) const
+      OptionalVariableIndex variableIndex) const {
-  {
     if (!variableIndex) {
-      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
+      // If no VariableIndex provided, compute one and call this function again
-      // for no variable index first so that it's always computed if we need to call COLAMD because
+      // IMPORTANT: we check for no variable index first so that it's always
-      // no Ordering is provided.  When removing optional from VariableIndex, create VariableIndex
+      // computed if we need to call COLAMD because no Ordering is provided.
-      // before creating ordering.
+      // When removing optional from VariableIndex, create VariableIndex before
+      // creating ordering.
       VariableIndex computedVariableIndex(asDerived());
-      return eliminateMultifrontal(function, computedVariableIndex, orderingType);
+      return eliminateMultifrontal(orderingType, function,
-    }
+                                   computedVariableIndex);
-    else {
+    } else {
-      // Compute an ordering and call this function again.  We are guaranteed to have a
+      // Compute an ordering and call this function again.  We are guaranteed to
-      // VariableIndex already here because we computed one if needed in the previous 'if' block.
+      // have a VariableIndex already here because we computed one if needed in
+      // the previous 'if' block.
       if (orderingType == Ordering::METIS) {
         Ordering computedOrdering = Ordering::Metis(asDerived());
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+        return eliminateMultifrontal(computedOrdering, function, variableIndex);
       } else {
         Ordering computedOrdering = Ordering::Colamd(*variableIndex);
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+        return eliminateMultifrontal(computedOrdering, function, variableIndex);
       }
     }
   }
@@ -273,7 +275,7 @@ namespace gtsam {
       else
       {
         // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-        return factorGraph->eliminateSequential(function);
+        return factorGraph->eliminateSequential(Ordering::COLAMD, function);
       }
     }
   }
@@ -340,7 +342,7 @@ namespace gtsam {
       else
       {
         // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-        return factorGraph->eliminateMultifrontal(function);
+        return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
       }
     }
   }

gtsam/inference/EliminateableFactorGraph.h

@@ -288,8 +288,9 @@ namespace gtsam {
     FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
 
   public:
+  #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
     /** \deprecated ordering and orderingType shouldn't both be specified */
-    boost::shared_ptr<BayesNetType> eliminateSequential(
+    boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
       const Ordering& ordering,
       const Eliminate& function,
       OptionalVariableIndex variableIndex,
@@ -298,7 +299,7 @@ namespace gtsam {
       }
     
     /** \deprecated orderingType specified first for consistency */
-    boost::shared_ptr<BayesNetType> eliminateSequential(
+    boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
       const Eliminate& function,
       OptionalVariableIndex variableIndex = boost::none,
       OptionalOrderingType orderingType = boost::none) const {
@@ -306,7 +307,7 @@ namespace gtsam {
       }
 
     /** \deprecated ordering and orderingType shouldn't both be specified */
-    boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
+    boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
       const Ordering& ordering,
       const Eliminate& function,
       OptionalVariableIndex variableIndex,
@@ -315,7 +316,7 @@ namespace gtsam {
       }
 
     /** \deprecated orderingType specified first for consistency */
-    boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
+    boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
       const Eliminate& function,
       OptionalVariableIndex variableIndex = boost::none,
       OptionalOrderingType orderingType = boost::none) const {
@@ -323,7 +324,7 @@ namespace gtsam {
       }
 
     /** \deprecated */
-    boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
+    boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
       boost::variant<const Ordering&, const KeyVector&> variables,
       boost::none_t,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
@@ -332,13 +333,14 @@ namespace gtsam {
       }
 
     /** \deprecated */
-    boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
+    boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
       boost::variant<const Ordering&, const KeyVector&> variables,
       boost::none_t,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const {
           return marginalMultifrontalBayesTree(variables, function, variableIndex);
       }
+  #endif
   };
 
 }

gtsam/inference/EliminationTree.h

@@ -81,7 +81,7 @@ namespace gtsam {
 
   protected:
     /** concept check */
-    GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
+    GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
 
     FastVector<sharedNode> roots_;
     FastVector<sharedFactor> remainingFactors_;

gtsam/inference/Factor.h

@@ -22,6 +22,7 @@
 #pragma once
 
 #include <boost/serialization/nvp.hpp>
+#include <boost/shared_ptr.hpp>
 
 #include <gtsam/base/types.h>
 #include <gtsam/base/FastVector.h>

gtsam/linear/Errors.cpp

@@ -111,10 +111,10 @@ double dot(const Errors& a, const Errors& b) {
 
 /* ************************************************************************* */
 template<>
-void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
+void axpy(double alpha, const Errors& x, Errors& y) {
   Errors::const_iterator it = x.begin();
   for(Vector& yi: y)
-    axpy(alpha,*(it++),yi);
+    yi += alpha * (*(it++));
 }
 
 /* ************************************************************************* */

gtsam/linear/Errors.h

@@ -66,7 +66,7 @@ namespace gtsam {
   * BLAS level 2 style
   */
   template <>
-  GTSAM_EXPORT void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y);
+  GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
 
   /** print with optional string */
   GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error");

gtsam/linear/GaussianFactorGraph.cpp

@@ -19,7 +19,6 @@
  */
 
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
@@ -290,10 +289,11 @@ namespace gtsam {
     return blocks;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
     gttic(GaussianFactorGraph_optimize);
-    return BaseEliminateable::eliminateMultifrontal(function)->optimize();
+    return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function)
+        ->optimize();
   }
 
   /* ************************************************************************* */
@@ -504,10 +504,32 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  /** \deprecated */
+  void GaussianFactorGraph::printErrors(
-  VectorValues GaussianFactorGraph::optimize(boost::none_t,
+      const VectorValues& values, const std::string& str,
-    const Eliminate& function) const {
+      const KeyFormatter& keyFormatter,
-      return optimize(function);
+      const std::function<bool(const Factor* /*factor*/,
+                               double /*whitenedError*/, size_t /*index*/)>&
+          printCondition) const {
+    cout << str << "size: " << size() << endl << endl;
+    for (size_t i = 0; i < (*this).size(); i++) {
+      const sharedFactor& factor = (*this)[i];
+      const double errorValue =
+          (factor != nullptr ? (*this)[i]->error(values) : .0);
+      if (!printCondition(factor.get(), errorValue, i))
+        continue;  // User-provided filter did not pass
+
+      stringstream ss;
+      ss << "Factor " << i << ": ";
+      if (factor == nullptr) {
+        cout << "nullptr"
+             << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        cout << "error = " << errorValue << "\n";
+      }
+      cout << endl;  // only one "endl" at end might be faster, \n for each
+                     // factor
+    }
   }
 
 } // namespace gtsam

gtsam/linear/GaussianFactorGraph.h

@@ -21,12 +21,13 @@
 
 #pragma once
 
-#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/EliminateableFactorGraph.h>
-#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/inference/FactorGraph.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/Errors.h>  // Included here instead of fw-declared so we can use Errors::iterator
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/VectorValues.h>
 
 namespace gtsam {
 
@@ -375,6 +376,14 @@ namespace gtsam {
     /** In-place version e <- A*x that takes an iterator. */
     void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
 
+    void printErrors(
+        const VectorValues& x,
+        const std::string& str = "GaussianFactorGraph: ",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter,
+        const std::function<bool(const Factor* /*factor*/,
+                                 double /*whitenedError*/, size_t /*index*/)>&
+            printCondition =
+                [](const Factor*, double, size_t) { return true; }) const;
     /// @}
 
   private:
@@ -387,9 +396,14 @@ namespace gtsam {
 
   public:
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
    /** \deprecated */
    VectorValues optimize(boost::none_t,
-      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
+                         const Eliminate& function =
+                             EliminationTraitsType::DefaultEliminate) const {
+     return optimize(function);
+   }
+#endif
 
   };
 

gtsam/linear/NoiseModel.h

@@ -177,17 +177,16 @@ namespace gtsam {
         return *sqrt_information_;
       }
 
-    protected:
-
-      /** protected constructor takes square root information matrix */
-      Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
-        Base(dim), sqrt_information_(sqrt_information) {
-      }
 
     public:
 
       typedef boost::shared_ptr<Gaussian> shared_ptr;
 
+      /** constructor takes square root information matrix */
+      Gaussian(size_t dim = 1,
+               const boost::optional<Matrix>& sqrt_information = boost::none)
+          : Base(dim), sqrt_information_(sqrt_information) {}
+
       ~Gaussian() override {}
 
       /**
@@ -290,13 +289,13 @@ namespace gtsam {
       Vector sigmas_, invsigmas_, precisions_;
 
     protected:
-      /** protected constructor - no initializations */
-      Diagonal();
 
       /** constructor to allow for disabling initialization of invsigmas */
       Diagonal(const Vector& sigmas);
 
     public:
+      /** constructor - no initializations, for serialization */
+      Diagonal();
 
       typedef boost::shared_ptr<Diagonal> shared_ptr;
 
@@ -387,14 +386,6 @@ namespace gtsam {
       // Sigmas are contained in the base class
       Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
 
-      /**
-       * protected constructor takes sigmas.
-       * prevents any inf values
-       * from appearing in invsigmas or precisions.
-       * mu set to large default value (1000.0)
-       */
-      Constrained(const Vector& sigmas = Z_1x1);
-
       /**
        * Constructor that prevents any inf values
        * from appearing in invsigmas or precisions.
@@ -406,6 +397,14 @@ namespace gtsam {
 
       typedef boost::shared_ptr<Constrained> shared_ptr;
 
+      /**
+       * protected constructor takes sigmas.
+       * prevents any inf values
+       * from appearing in invsigmas or precisions.
+       * mu set to large default value (1000.0)
+       */
+      Constrained(const Vector& sigmas = Z_1x1);
+
       ~Constrained() override {}
 
       /// true if a constrained noise mode, saves slow/clumsy dynamic casting
@@ -461,6 +460,11 @@ namespace gtsam {
         return MixedVariances(precisions.array().inverse());
       }
 
+      /**
+       * The squaredMahalanobisDistance function for a constrained noisemodel,
+       * for non-constrained versions, uses sigmas, otherwise
+       * uses the penalty function with mu
+       */
       double squaredMahalanobisDistance(const Vector& v) const override;
 
       /** Fully constrained variations */
@@ -531,11 +535,11 @@ namespace gtsam {
       Isotropic(size_t dim, double sigma) :
         Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
 
+    public:
+
       /* dummy constructor to allow for serialization */
       Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
 
-    public:
-
       ~Isotropic() override {}
 
       typedef boost::shared_ptr<Isotropic> shared_ptr;
@@ -592,14 +596,13 @@ namespace gtsam {
      * Unit: i.i.d. unit-variance noise on all m dimensions.
      */
     class GTSAM_EXPORT Unit : public Isotropic {
-    protected:
-
-      Unit(size_t dim=1): Isotropic(dim,1.0) {}
-
     public:
 
       typedef boost::shared_ptr<Unit> shared_ptr;
 
+      /** constructor for serialization */
+      Unit(size_t dim=1): Isotropic(dim,1.0) {}
+
       ~Unit() override {}
 
       /**
@@ -682,19 +685,19 @@ namespace gtsam {
       /// Return the contained noise model
       const NoiseModel::shared_ptr& noise() const { return noise_; }
 
-      // TODO: functions below are dummy but necessary for the noiseModel::Base
+      // Functions below are dummy but necessary for the noiseModel::Base
       inline Vector whiten(const Vector& v) const override
       { Vector r = v; this->WhitenSystem(r); return r; }
       inline Matrix Whiten(const Matrix& A) const override
       { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
       inline Vector unwhiten(const Vector& /*v*/) const override
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
-
+      /// Compute loss from the m-estimator using the Mahalanobis distance.
       double loss(const double squared_distance) const override {
         return robust_->loss(std::sqrt(squared_distance));
       }
 
-      // TODO: these are really robust iterated re-weighting support functions
+      // These are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
       void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
       void WhitenSystem(Matrix& A, Vector& b) const override;
@@ -705,7 +708,6 @@ namespace gtsam {
         return noise_->unweightedWhiten(v);
       }
       double weight(const Vector& v) const override {
-        // Todo(mikebosse): make the robust weight function input a vector.
         return robust_->weight(v.norm());
       }
 

gtsam/linear/SubgraphPreconditioner.cpp

@@ -173,7 +173,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd
   Errors::const_iterator it = e.begin();
   for(auto& key_value: y) {
     const Vector& ei = *it;
-    axpy(alpha, ei, key_value.second);
+    key_value.second += alpha * ei;
     ++it;
   }
   transposeMultiplyAdd2(alpha, it, e.end(), y);
@@ -191,7 +191,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
 
   VectorValues x = VectorValues::Zero(y); // x = 0
   Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
-  axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
+  y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x
 }
 
 /* ************************************************************************* */

gtsam/linear/iterative-inl.h

@@ -72,7 +72,7 @@ namespace gtsam {
     double takeOptimalStep(V& x) {
       // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial
       double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
-      axpy(alpha, d, x); // // do step in new search direction, x += alpha*d
+      x += alpha * d; // do step in new search direction, x += alpha*d
       return alpha;
     }
 
@@ -106,7 +106,7 @@ namespace gtsam {
         double beta = new_gamma / gamma;
         // d = g + d*beta;
         d *= beta;
-        axpy(1.0, g, d);
+        d += 1.0 * g;
       }
 
       gamma = new_gamma;

gtsam/linear/linear.i

@@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Diagonal : gtsam::noiseModel::Gaussian {
@@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Constrained : gtsam::noiseModel::Diagonal {
@@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
 
     // enabling serialization functionality
     void serializable() const;
+
+    // enable pickling in python
+    void pickle() const;
 };
 
 virtual class Isotropic : gtsam::noiseModel::Diagonal {
@@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Unit : gtsam::noiseModel::Isotropic {
@@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 namespace mEstimator {
@@ -221,6 +236,7 @@ class VectorValues {
   //Constructors
   VectorValues();
   VectorValues(const gtsam::VectorValues& other);
+  VectorValues(const gtsam::VectorValues& first, const gtsam::VectorValues& second);
 
   //Named Constructors
   static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
@@ -301,6 +317,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
   void printKeys(string s) const;
+  gtsam::KeyVector& keys() const;
   bool equals(const gtsam::GaussianFactor& lf, double tol) const;
   size_t size() const;
   Vector unweighted_error(const gtsam::VectorValues& c) const;
@@ -400,6 +417,7 @@ class GaussianFactorGraph {
   // error and probability
   double error(const gtsam::VectorValues& c) const;
   double probPrime(const gtsam::VectorValues& c) const;
+  void printErrors(const gtsam::VectorValues& c, string str = "GaussianFactorGraph: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
 
   gtsam::GaussianFactorGraph clone() const;
   gtsam::GaussianFactorGraph negate() const;
@@ -512,9 +530,9 @@ virtual class GaussianBayesNet {
   size_t size() const;
 
   // FactorGraph derived interface
-  // size_t size() const;
   gtsam::GaussianConditional* at(size_t idx) const;
   gtsam::KeySet keys() const;
+  gtsam::KeyVector keyVector() const;
   bool exists(size_t idx) const;
 
   void saveGraph(const string& s) const;

gtsam/linear/tests/testNoiseModel.cpp

@@ -665,22 +665,11 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
       noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
       Unit::Create(3));
 
-/*
+  for (int i = 0; i < 5; i++) {
- * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
+    Vector3 error = Vector3(i, 0, 0);
- * implement a loss function, and GTSAM calls the weight function to evaluate the
+    DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
- * total penalty, rather than calling the loss function. The weight function should be
+                  robust->squaredMahalanobisDistance(error), 1e-8);
- * used during iteratively reweighted least squares optimization, but should not be used to
+  }
- * evaluate the total penalty. The long-term solution is for all mEstimators to implement
- * both a weight and a loss function, and for GTSAM to call the loss function when
- * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
- * commented out until the underlying bug in GTSAM is fixed.
- *
- * for (int i = 0; i < 5; i++) {
- *   Vector3 error = Vector3(i, 0, 0);
- *   DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8);
- * }
- */
-
 }
 
 TEST(NoiseModel, lossFunctionAtZero)
@@ -707,9 +696,9 @@ TEST(NoiseModel, lossFunctionAtZero)
   auto dcs = mEstimator::DCS::Create(k);
   DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
   DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
-  // auto lsdz = mEstimator::L2WithDeadZone::Create(k);
+  auto lsdz = mEstimator::L2WithDeadZone::Create(k);
-  // DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
-  // DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8);
+  DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8);
 }
 
 

gtsam/linear/tests/testSerializationLinear.cpp

@@ -39,14 +39,14 @@ using namespace gtsam::serializationTestHelpers;
 /* ************************************************************************* */
 // Export Noisemodels
 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
 
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
 
 /* ************************************************************************* */
 // example noise models
@@ -129,9 +129,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
 
 /* Create GUIDs for factors */
 /* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
-BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
-BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
+BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
 
 /* ************************************************************************* */
 TEST (Serialization, linear_factors) {

gtsam/navigation/CombinedImuFactor.cpp

@@ -257,5 +257,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
  /// namespace gtsam
 
 /// Boost serialization export definition for derived class
-BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams);
+BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams)
 

gtsam/navigation/CombinedImuFactor.h

@@ -353,4 +353,4 @@ struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
 }  // namespace gtsam
 
 /// Add Boost serialization export key (declaration) for derived class
-BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams);
+BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)

gtsam/navigation/navigation.i

@@ -41,6 +41,12 @@ class ConstantBias {
   Vector gyroscope() const;
   Vector correctAccelerometer(Vector measurement) const;
   Vector correctGyroscope(Vector measurement) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 }///\namespace imuBias
@@ -64,6 +70,12 @@ class NavState {
 
   gtsam::NavState retract(const Vector& x) const;
   Vector localCoordinates(const gtsam::NavState& g) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 #include <gtsam/navigation/PreintegratedRotation.h>
@@ -88,6 +100,8 @@ virtual class PreintegratedRotationParams {
 virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
 
+  gtsam::Vector n_gravity;
+
   static gtsam::PreintegrationParams* MakeSharedD(double g);
   static gtsam::PreintegrationParams* MakeSharedU(double g);
   static gtsam::PreintegrationParams* MakeSharedD();  // default g = 9.81
@@ -104,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   Matrix getAccelerometerCovariance() const;
   Matrix getIntegrationCovariance()   const;
   bool   getUse2ndOrderCoriolis()     const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 #include <gtsam/navigation/ImuFactor.h>
@@ -133,6 +153,12 @@ class PreintegratedImuMeasurements {
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class ImuFactor: gtsam::NonlinearFactor {

gtsam/navigation/tests/testAttitudeFactor.cpp

@@ -66,7 +66,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
 /* ************************************************************************* */
 // Export Noisemodels
 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
 
 /* ************************************************************************* */
 TEST(Rot3AttitudeFactor, Serialization) {

gtsam/navigation/tests/testGPSFactor.cpp

@@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Pose3>(
-      std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
+      std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
@@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,NavState>(
-      std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
+      std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;

gtsam/navigation/tests/testImuFactorSerialization.cpp

@@ -31,16 +31,16 @@ using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
-                        "gtsam_noiseModel_Constrained");
+                        "gtsam_noiseModel_Constrained")
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
-                        "gtsam_noiseModel_Diagonal");
+                        "gtsam_noiseModel_Diagonal")
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
-                        "gtsam_noiseModel_Gaussian");
+                        "gtsam_noiseModel_Gaussian")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
-                        "gtsam_noiseModel_Isotropic");
+                        "gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
 
 template <typename P>
 P getPreintegratedMeasurements() {

gtsam/navigation/tests/testMagFactor.cpp

@@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) {
   Point3 expected(22735.5, 314.502, 44202.5);
   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
   EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
-      (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
+      (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
 }
 
 // *************************************************************************
@@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) {
   MagFactor f(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
   EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
-      (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
+      (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
 
 // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
-      (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
+      (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
 
 // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
   EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
+      (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
       H1, 1e-7));
   EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
+      (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
       H2, 1e-7));
 
 // MagFactor2
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
-      (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
+      (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
       H1, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
-      (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
       H2, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
       H3, 1e-7));
 }
 

gtsam/nonlinear/CustomFactor.h

@@ -101,4 +101,4 @@ private:
   }
 };
 
-};
+}

gtsam/nonlinear/DoglegOptimizerImpl.cpp

@@ -78,7 +78,8 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues&
 
   // Compute blended point
   if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
-  VectorValues blend = (1. - tau) * x_u;  axpy(tau, x_n, blend);
+  VectorValues blend = (1. - tau) * x_u;
+  blend += tau * x_n;
   return blend;
 }
 

gtsam/nonlinear/Expression-inl.h

@@ -246,6 +246,18 @@ struct apply_compose {
     return x.compose(y, H1, H2);
   }
 };
+
+template <>
+struct apply_compose<double> {
+  double operator()(const double& x, const double& y,
+                    OptionalJacobian<1, 1> H1 = boost::none,
+                    OptionalJacobian<1, 1> H2 = boost::none) const {
+    if (H1) H1->setConstant(y);
+    if (H2) H2->setConstant(x);
+    return x * y;
+  }
+};
+
 }
 
 // Global methods:

gtsam/nonlinear/ExpressionFactor.h

@@ -305,7 +305,7 @@ struct traits<ExpressionFactorN<T, Args...>>
  * \deprecated Prefer the more general ExpressionFactorN<>.
  */
 template <typename T, typename A1, typename A2>
-class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
+class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
 public:
   /// Destructor
   ~ExpressionFactor2() override {}

gtsam/nonlinear/GncOptimizer.h

@@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer {
    * provides an extra interface for the user to initialize the weightst
    * */
   void setWeights(const Vector w) {
-    if(w.size() != nfg_.size()){
+    if (size_t(w.size()) != nfg_.size()) {
-      throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights"
+      throw std::runtime_error(
+          "GncOptimizer::setWeights: the number of specified weights"
           " does not match the size of the factor graph.");
     }
     weights_ = w;

gtsam/nonlinear/LinearContainerFactor.h

@@ -29,9 +29,6 @@ protected:
   GaussianFactor::shared_ptr factor_;
   boost::optional<Values> linearizationPoint_;
 
-  /** Default constructor - necessary for serialization */
-  LinearContainerFactor() {}
-
   /** direct copy constructor */
   GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
 
@@ -43,6 +40,9 @@ public:
 
   typedef boost::shared_ptr<This> shared_ptr;
 
+  /** Default constructor - necessary for serialization */
+  LinearContainerFactor() {}
+
   /** Primary constructor: store a linear factor with optional linearization point */
   GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
 

gtsam/nonlinear/Marginals.cpp

@@ -80,11 +80,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut
 
 /* ************************************************************************* */
 void Marginals::computeBayesTree() {
+  // The default ordering to use.
+  const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD;
   // Compute BayesTree
   if (factorization_ == CHOLESKY)
-    bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky);
+    bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType,
+                                               EliminatePreferCholesky);
   else if (factorization_ == QR)
-    bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR);
+    bayesTree_ =
+        *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/Marginals.h

@@ -131,17 +131,19 @@ protected:
   void computeBayesTree(const Ordering& ordering);
 
 public:
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /** \deprecated argument order changed due to removing boost::optional<Ordering> */
-  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
+  GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
             const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
 
   /** \deprecated argument order changed due to removing boost::optional<Ordering> */
-  Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
+  GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
             const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
 
   /** \deprecated argument order changed due to removing boost::optional<Ordering> */
-  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
+  GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
             const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
+#endif
 
 };
 

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -149,7 +149,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
     const V &initial, const NonlinearOptimizerParams &params,
     const bool singleIteration, const bool gradientDescent = false) {
 
-  // GTSAM_CONCEPT_MANIFOLD_TYPE(V);
+  // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
 
   size_t iteration = 0;
 

gtsam/nonlinear/NonlinearEquality.h

@@ -219,7 +219,6 @@ protected:
   X value_; /// fixed value for variable
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(X)
-
   GTSAM_CONCEPT_TESTABLE_TYPE(X)
 
 public:

gtsam/nonlinear/NonlinearFactor.cpp

@@ -114,7 +114,7 @@ double NoiseModelFactor::weight(const Values& c) const {
     if (noiseModel_) {
       const Vector b = unwhitenedError(c);
       check(noiseModel_, b.size());
-      return 0.5 * noiseModel_->weight(b);
+      return noiseModel_->weight(b);
     }
     else
       return 1.0;

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -265,15 +265,17 @@ namespace gtsam {
 
   public:
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
     /** \deprecated */
-    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
+    boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
         const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
       {return linearizeToHessianFactor(values, dampen);}
 
     /** \deprecated */
-    Values updateCholesky(const Values& values, boost::none_t,
+    Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
                           const Dampen& dampen = nullptr) const
       {return updateCholesky(values, dampen);}
+#endif
 
   };
 

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
     if (params.ordering)
-      delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
+      delta = gfg.eliminateSequential(*params.ordering,
-                                      boost::none, params.orderingType)->optimize();
+                                      params.getEliminationFunction())
+                  ->optimize();
     else
-      delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none,
+      delta = gfg.eliminateSequential(params.orderingType,
-                                      params.orderingType)->optimize();
+                                      params.getEliminationFunction())
+                  ->optimize();
   } else if (params.isIterative()) {
     // Conjugate Gradient -> needs params.iterativeParams
     if (!params.iterativeParams)

gtsam/nonlinear/Values.h

@@ -24,6 +24,7 @@
 
 #pragma once
 
+#include <gtsam/base/FastDefaultAllocator.h>
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/inference/Key.h>
@@ -62,17 +63,18 @@ namespace gtsam {
   class GTSAM_EXPORT Values {
 
   private:
-
     // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
-    // below) to clone and deallocate the Value objects, and a boost
+    // below) to clone and deallocate the Value objects, and our compile-flag-
-    // fast_pool_allocator to allocate map nodes.  In this way, all memory is
+    // dependent FastDefaultAllocator to allocate map nodes.  In this way, the
-    // allocated in a boost memory pool.
+    // user defines the allocation details (i.e. optimize for memory pool/arenas
+    // concurrency).
+    typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
     typedef boost::ptr_map<
         Key,
         Value,
         std::less<Key>,
         ValueCloneAllocator,
-        boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
+        KeyValuePtrPairAllocator > KeyValueMap;
 
     // The member to store the values, see just above
     KeyValueMap values_;

gtsam/nonlinear/factorTesting.h

@@ -36,7 +36,6 @@ namespace gtsam {
  */
 JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
                                     const Values& values, double delta = 1e-5) {
-
   // We will fill a vector of key/Jacobians pairs (a map would sort)
   std::vector<std::pair<Key, Matrix> > jacobians;
 
@@ -53,11 +52,11 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
     Matrix J = Matrix::Zero(rows, cols);
     for (size_t col = 0; col < cols; ++col) {
       Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
-      dx[col] = delta;
+      dx(col) = delta;
       dX[key] = dx;
       Values eval_values = values.retract(dX);
       const Eigen::VectorXd left = factor.whitenedError(eval_values);
-      dx[col] = -delta;
+      dx(col) = -delta;
       dX[key] = dx;
       eval_values = values.retract(dX);
       const Eigen::VectorXd right = factor.whitenedError(eval_values);

gtsam/nonlinear/nonlinear.i

@@ -115,6 +115,10 @@ class Ordering {
   Ordering();
   Ordering(const gtsam::Ordering& other);
 
+  template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
+                            gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
+
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
@@ -734,7 +738,12 @@ class ISAM2 {
                             const gtsam::KeyList& extraReelimKeys,
                             bool force_relinearize);
 
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::ISAM2UpdateParams& updateParams);
+
   gtsam::Values getLinearizationPoint() const;
+  bool valueExists(gtsam::Key key) const;
   gtsam::Values calculateEstimate() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@@ -744,12 +753,17 @@ class ISAM2 {
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                      gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
-  gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
+  gtsam::Values calculateBestEstimate() const;
   gtsam::VectorValues getDelta() const;
+  double error(const gtsam::VectorValues& x) const;
   gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
   gtsam::VariableIndex getVariableIndex() const;
+  const gtsam::KeySet& getFixedVariables() const;
   gtsam::ISAM2Params params() const;
+
+  void printStats() const;
+  gtsam::VectorValues gradientAtZero() const;
 };
 
 #include <gtsam/nonlinear/NonlinearISAM.h>

gtsam/nonlinear/tests/testExpression.cpp

@@ -293,6 +293,19 @@ TEST(Expression, compose3) {
   EXPECT(expected == R3.keys());
 }
 
+/* ************************************************************************* */
+// Test compose with double type (should be multiplication).
+TEST(Expression, compose4) {
+  // Create expression
+  gtsam::Key key = 1;
+  Double_ R1(key), R2(key);
+  Double_ R3 = R1 * R2;
+
+  // Check keys
+  set<Key> expected = list_of(1);
+  EXPECT(expected == R3.keys());
+}
+
 /* ************************************************************************* */
 // Test with ternary function.
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -35,37 +35,37 @@ using namespace gtsam::serializationTestHelpers;
 
 /* ************************************************************************* */
 // Create GUIDs for Noisemodels
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
 
 /* ************************************************************************* */
 // Create GUIDs for factors
-BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
+BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>")
-BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
-BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
-BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
+BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
 
 
 /* ************************************************************************* */
 // Export all classes derived from Value
-GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
+GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
-GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler);
+GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler)
-GTSAM_VALUE_EXPORT(gtsam::Point3);
+GTSAM_VALUE_EXPORT(gtsam::Point3)
-GTSAM_VALUE_EXPORT(gtsam::Pose3);
+GTSAM_VALUE_EXPORT(gtsam::Pose3)
-GTSAM_VALUE_EXPORT(gtsam::Rot3);
+GTSAM_VALUE_EXPORT(gtsam::Rot3)
-GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>);
+GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
-GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>);
+GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
-GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>);
+GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
 
 namespace detail {
 template<class T> struct pack {

gtsam/slam/BetweenFactor.h

@@ -103,7 +103,7 @@ namespace gtsam {
       boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
-#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR
+#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
       typename traits<T>::ChartJacobian::Jacobian Hlocal;
       Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
       if (H1) *H1 = Hlocal * (*H1);

gtsam/slam/GeneralSFMFactor.h

@@ -59,8 +59,8 @@ namespace gtsam {
 template<class CAMERA, class LANDMARK>
 class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
-  GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
 
   static const int DimC = FixedDimension<CAMERA>::value;
   static const int DimL = FixedDimension<LANDMARK>::value;
@@ -202,7 +202,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
 template<class CALIBRATION>
 class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
   static const int DimK = FixedDimension<CALIBRATION>::value;
 
 protected:

gtsam/slam/JacobianFactorSVD.h

@@ -9,20 +9,21 @@
 
 namespace gtsam {
 /**
- * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis
+ * JacobianFactor for Schur complement that uses the "Nullspace Trick" by
+ * Mourikis et al.
  *
  * This trick is equivalent to the Schur complement, but can be faster.
- * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses,
+ * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
- * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e.,
+ * poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
- * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3)
+ * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
- * where Enull is an m x (m-3) matrix
+ * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
- * Then Enull'*E*dp = 0, and
  *  |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b|
  * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
  *
- * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks.
+ * The code below assumes that F is block diagonal and is given as a vector of
- * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6)
+ * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
- * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult
+ * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as
+ * a 1x2 * 2x6 multiplication.
  */
 template<size_t D, size_t ZDim>
 class JacobianFactorSVD: public RegularJacobianFactor<D> {
@@ -37,10 +38,10 @@ public:
   JacobianFactorSVD() {
   }
 
-  /// Empty constructor with keys
+  /// Empty constructor with keys.
-  JacobianFactorSVD(const KeyVector& keys, //
+  JacobianFactorSVD(const KeyVector& keys,
-      const SharedDiagonal& model = SharedDiagonal()) :
+                    const SharedDiagonal& model = SharedDiagonal())
-      Base() {
+      : Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
@@ -51,18 +52,21 @@ public:
   }
 
   /**
-   * @brief Constructor
+   * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
-   * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
+   * Jacobian factor on the CameraSet.
-   * and a reduced point derivative, Enull
-   * and creates a reduced-rank Jacobian factor on the CameraSet
    *
-   * @Fblocks:
+   * @param keys keys associated with F blocks.
+   * @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F
+   * @param Enull a reduced point derivative
+   * @param b right-hand side
+   * @param model noise model
    */
-  JacobianFactorSVD(const KeyVector& keys,
+  JacobianFactorSVD(
-      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
+      const KeyVector& keys,
-      const Vector& b, //
+      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
-      const SharedDiagonal& model = SharedDiagonal()) :
+      const Matrix& Enull, const Vector& b,
-      Base() {
+      const SharedDiagonal& model = SharedDiagonal())
+      : Base() {
     size_t numKeys = Enull.rows() / ZDim;
     size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
     // PLAIN nullptr SPACE TRICK
@@ -74,9 +78,8 @@ public:
     QF.reserve(numKeys);
     for (size_t k = 0; k < Fblocks.size(); ++k) {
       Key key = keys[k];
-      QF.push_back(
+      QF.emplace_back(
-          KeyMatrix(key,
+          key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
-              (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
     }
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }

gtsam/slam/ProjectionFactor.h

@@ -11,7 +11,7 @@
 
 /**
  * @file ProjectionFactor.h
- * @brief Basic bearing factor from 2D measurement
+ * @brief Reprojection of a LANDMARK to a 2D point.
  * @author Chris Beall
  * @author Richard Roberts
  * @author Frank Dellaert
@@ -22,17 +22,21 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
 
   /**
-   * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
+   * Non-linear factor for a constraint derived from a 2D measurement. 
-   * i.e. the main building block for visual SLAM.
+   * The calibration is known here.
+   * The main building block for visual SLAM.
    * @addtogroup SLAM
    */
-  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
+  template <class POSE = Pose3, class LANDMARK = Point3,
+            class CALIBRATION = Cal3_S2>
   class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
   protected: