Merge remote-tracking branch 'origin/develop' into fan/prototype-hybrid-tr

.github/workflows/build-windows.yml

@@ -48,7 +48,9 @@ jobs:
       - name: Install Dependencies
         shell: powershell
         run: |
-          Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
+          iwr -useb get.scoop.sh -outfile 'install_scoop.ps1'
+          .\install_scoop.ps1 -RunAsAdmin
+
           scoop install cmake --global  # So we don't get issues with CMP0074 policy
           scoop install ninja --global
 

.gitignore

@@ -17,3 +17,4 @@
 # for QtCreator:
 CMakeLists.txt.user*
 xcode/
+/Dockerfile

CMakeLists.txt

@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a5")
+set (GTSAM_PRERELEASE_VERSION "a6")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

INSTALL.md

@@ -13,7 +13,7 @@ $ make install
 ## Important Installation Notes
 
 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes).
+    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes) for version recommendations based on your compiler.
 
     - Cmake version 3.0 or higher
     - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
@@ -72,7 +72,7 @@ execute commands as follows for an out-of-source build:
 Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized.
 This is particularly seen when using `clang` as the C++ compiler.
 
-For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
+For this reason we recommend Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
 
 ## Known Issues
 

cmake/GtsamBuildTypes.cmake

@@ -191,7 +191,7 @@ endif()
 
 if (NOT MSVC)
   option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
-  if(GTSAM_BUILD_WITH_MARCH_NATIVE)
+  if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
     # Add as public flag so all dependant projects also use it, as required
     # by Eigen to avid crashes due to SIMD vectorization:
     list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")

cmake/HandleMetis.cmake

@@ -21,7 +21,12 @@ if(GTSAM_USE_SYSTEM_METIS)
     mark_as_advanced(METIS_LIBRARY)
 
     add_library(metis-gtsam-if INTERFACE)
-    target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR})
+    target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}
+      # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API
+      # via extern "C"
+      $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
+      $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
+    )
     target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY})
   endif()
 else()
@@ -30,10 +35,12 @@ else()
   add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis)
 
   target_include_directories(metis-gtsam BEFORE PUBLIC
+    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
+    # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API
+    # via extern "C"
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
-    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
   )
 
   add_library(metis-gtsam-if INTERFACE)

doc/Code/LocalizationExample2.cpp

@@ -1,7 +1,7 @@
 // add unary measurement factors, like GPS, on all three poses
-noiseModel::Diagonal::shared_ptr unaryNoise =
+auto unaryNoise =
  noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
-graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
 

doc/Code/LocalizationFactor.cpp

@@ -6,8 +6,7 @@ public:
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
   Vector evaluateError(const Pose2& q, 
-                       boost::optional<Matrix&> H = boost::none) const
+      boost::optional<Matrix&> H = boost::none) const override {
-  {
     const Rot2& R = q.rotation();
     if (H) (*H) = (gtsam::Matrix(2, 3) <<
             R.c(), -R.s(), 0.0,

doc/Code/OdometryExample.cpp

@@ -3,13 +3,11 @@ NonlinearFactorGraph graph;
 
 // Add a Gaussian prior on pose x_1
 Pose2 priorMean(0.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr priorNoise =
+auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
-graph.addPrior(1, priorMean, priorNoise);
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr odometryNoise =
+auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

doc/Code/OdometryOutput1.txt

@@ -1,11 +1,14 @@
 Factor Graph:
 size: 3
-factor 0: PriorFactor on 1
+
-  prior mean: (0, 0, 0)
+Factor 0: PriorFactor on 1
+  prior mean:  (0, 0, 0)
   noise model: diagonal sigmas [0.3; 0.3; 0.1];
-factor 1: BetweenFactor(1,2)
+
-  measured: (2, 0, 0)
+Factor 1: BetweenFactor(1,2)
+  measured:  (2, 0, 0)
   noise model: diagonal sigmas [0.2; 0.2; 0.1];
-factor 2: BetweenFactor(2,3)
+
-  measured: (2, 0, 0)
+Factor 2: BetweenFactor(2,3)
+  measured:  (2, 0, 0)
   noise model: diagonal sigmas [0.2; 0.2; 0.1];

doc/Code/OdometryOutput2.txt

@@ -1,11 +1,23 @@
 Initial Estimate:
+
 Values with 3 values:
-Value 1: (0.5, 0, 0.2)
+Value 1: (gtsam::Pose2)
-Value 2: (2.3, 0.1, -0.2)
+(0.5, 0, 0.2)
-Value 3: (4.1, 0.1, 0.1)
+
+Value 2: (gtsam::Pose2)
+(2.3, 0.1, -0.2)
+
+Value 3: (gtsam::Pose2)
+(4.1, 0.1, 0.1)
 
 Final Result:
+
 Values with 3 values:
-Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
+Value 1: (gtsam::Pose2)
-Value 2: (2, 7.4e-18, -2.5e-18)
+(7.5-16, -5.3-16, -1.8-16)
-Value 3: (4, -1.8e-18, -3.1e-18)
+
+Value 2: (gtsam::Pose2)
+(2, -1.1-15, -2.5-16)
+
+Value 3: (gtsam::Pose2)
+(4, -1.7-15, -2.5-16)

doc/Code/OdometryOutput3.txt

@@ -1,12 +1,12 @@
 x1 covariance:
-       0.09     1.1e-47     5.7e-33
+   0.09 1.7e-33 2.8e-33
-    1.1e-47        0.09     1.9e-17
+1.7e-33    0.09 2.6e-17
-    5.7e-33     1.9e-17        0.01
+2.8e-33 2.6e-17    0.01
 x2 covariance:
-       0.13     4.7e-18     2.4e-18
+   0.13 1.2e-18 6.1e-19
-    4.7e-18        0.17        0.02
+1.2e-18    0.17    0.02
-    2.4e-18        0.02        0.02
+6.1e-19    0.02    0.02
 x3 covariance:
-       0.17     2.7e-17     8.4e-18
+   0.17 8.6e-18 2.7e-18
-    2.7e-17        0.37        0.06
+8.6e-18    0.37    0.06
-    8.4e-18        0.06        0.03
+2.7e-18    0.06    0.03

doc/gtsam.lyx

@@ -134,14 +134,10 @@ A Hands-on Introduction
 
 \begin_layout Author
 Frank Dellaert
-\begin_inset Newline newline
-\end_inset
-
-Technical Report number GT-RIM-CP&R-2014-XXX
 \end_layout
 
 \begin_layout Date
-September 2014
+Updated Last March 2022
 \end_layout
 
 \begin_layout Standard
@@ -154,18 +150,14 @@ filename "common_macros.tex"
 
 \end_layout
 
-\begin_layout Section*
+\begin_layout Abstract
-Overview
-\end_layout
-
-\begin_layout Standard
 In this document I provide a hands-on introduction to both factor graphs
  and GTSAM.
  This is an updated version from the 2012 TR that is tailored to our GTSAM
- 3.0 library and beyond.
+ 4.0 library and beyond.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 
 \series bold
 Factor graphs
@@ -206,7 +198,7 @@ ts or prior knowledge.
  robotics and vision.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 The GTSAM toolbox (GTSAM stands for 
 \begin_inset Quotes eld
 \end_inset
@@ -221,11 +213,13 @@ Georgia Tech Smoothing and Mapping
  It provides state of the art solutions to the SLAM and SFM problems, but
  can also be used to model and solve both simpler and more complex estimation
  problems.
- It also provides a MATLAB interface which allows for rapid prototype developmen
+ It also provides MATLAB and Python wrappers which allow for rapid prototype
-t, visualization, and user interaction.
+ development, visualization, and user interaction.
+ In addition, it is easy to use in Jupyter notebooks and/or Google's coLaborator
+y.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 GTSAM exploits sparsity to be computationally efficient.
  Typically measurements only provide information on the relationship between
  a handful of variables, and hence the resulting factor graph will be sparsely
@@ -236,14 +230,17 @@ l complexity.
  GTSAM provides iterative methods that are quite efficient regardless.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
-You can download the latest version of GTSAM at 
+You can download the latest version of GTSAM from GitHub at
+\end_layout
+
+\begin_layout Abstract
 \begin_inset Flex URL
 status open
 
 \begin_layout Plain Layout
 
-http://tinyurl.com/gtsam
+https://github.com/borglab/gtsam
 \end_layout
 
 \end_inset
@@ -741,7 +738,7 @@ noindent
 \begin_inset Formula $f_{0}(x_{1})$
 \end_inset
 
- on lines 5-8 as an instance of 
+ on lines 5-7 as an instance of 
 \series bold
 \emph on
 PriorFactor<T>
@@ -773,7 +770,7 @@ Pose2,
 noiseModel::Diagonal
 \series default
 \emph default
- by specifying three standard deviations in line 7, respectively 30 cm.
+ by specifying three standard deviations in line 6, respectively 30 cm.
 \begin_inset space ~
 \end_inset
 
@@ -795,7 +792,7 @@ Similarly, odometry measurements are specified as
 Pose2
 \series default
 \emph default
- on line 11, with a slightly different noise model defined on line 12-13.
+ on line 10, with a slightly different noise model defined on line 11.
  We then add the two factors 
 \begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$
 \end_inset
@@ -804,7 +801,7 @@ Pose2
 \begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$
 \end_inset
 
- on lines 14-15, as instances of yet another templated class, 
+ on lines 12-13, as instances of yet another templated class, 
 \series bold
 \emph on
 BetweenFactor<T>
@@ -875,7 +872,7 @@ smoothing and mapping
 
 .
  Later in this document we will talk about how we can also use GTSAM to
- do filtering (which you often do 
+ do filtering (which often you do 
 \emph on
 not
 \emph default
@@ -928,7 +925,11 @@ Values
 \begin_layout Standard
 The latter point is often a point of confusion with beginning users of GTSAM.
  It helps to remember that when designing GTSAM we took a functional approach
- of classes corresponding to mathematical objects, which are usually immutable.
+ of classes corresponding to mathematical objects, which are usually 
+\emph on
+immutable
+\emph default
+.
  You should think of a factor graph as a 
 \emph on
 function
@@ -1363,14 +1364,18 @@ where
 \end_inset
 
  is the measurement, 
-\begin_inset Formula $q$
+\begin_inset Formula $q\in SE(2)$
 \end_inset
 
  is the unknown variable, 
 \begin_inset Formula $h(q)$
 \end_inset
 
- is a (possibly nonlinear) measurement function, and 
+ is a 
+\series bold
+measurement function
+\series default
+, and 
 \begin_inset Formula $\Sigma$
 \end_inset
 
@@ -1546,7 +1551,7 @@ E(q)\define h(q)-m
 
 \end_inset
 
-which is done on line 12.
+which is done on line 14.
  Importantly, because we want to use this factor for nonlinear optimization
  (see e.g., 
 \begin_inset CommandInset citation
@@ -1599,11 +1604,11 @@ q_{y}
 \begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$
 \end_inset
 
-, yields the following simple 
+, yields the following 
 \begin_inset Formula $2\times3$
 \end_inset
 
- matrix in tangent space which is the same the as the rotation matrix:
+ matrix:
 \end_layout
 
 \begin_layout Standard
@@ -1618,6 +1623,171 @@ H=\left[\begin{array}{ccc}
 \end_inset
 
 
+\end_layout
+
+\begin_layout Paragraph*
+Important Note
+\end_layout
+
+\begin_layout Standard
+Many of our users, when attempting to create a custom factor, are initially
+ surprised at the Jacobian matrix not agreeing with their intuition.
+ For example, above you might simply expect a 
+\begin_inset Formula $2\times3$
+\end_inset
+
+ identity matrix.
+ This 
+\emph on
+would
+\emph default
+ be true for variables belonging to a vector space.
+ However, in GTSAM we define the Jacobian more generally to be the matrix
+ 
+\begin_inset Formula $H$
+\end_inset
+
+ such that
+\begin_inset Formula 
+\[
+h(q\exp\hat{\xi})\approx h(q)+H\xi
+\]
+
+\end_inset
+
+where 
+\begin_inset Formula $\xi=(\delta x,\delta y,\delta\theta)$
+\end_inset
+
+ is an incremental update and 
+\begin_inset Formula $\exp\hat{\xi}$
+\end_inset
+
+ is the 
+\series bold
+exponential map
+\series default
+ for the variable we want to update.
+ In this case 
+\begin_inset Formula $q\in SE(2)$
+\end_inset
+
+, where 
+\begin_inset Formula $SE(2)$
+\end_inset
+
+ is the group of 2D rigid transforms, implemented by 
+\series bold
+\emph on
+Pose2
+\emph default
+.
+ 
+\series default
+The exponential map for 
+\begin_inset Formula $SE(2)$
+\end_inset
+
+ can be approximated to first order as
+\begin_inset Formula 
+\[
+\exp\hat{\xi}\approx\left[\begin{array}{ccc}
+1 & -\delta\theta & \delta x\\
+\delta\theta & 1 & \delta y\\
+0 & 0 & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+when using the 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ matrix representation for 2D poses, and hence 
+\begin_inset Formula 
+\[
+h(qe^{\hat{\xi}})\approx h\left(\left[\begin{array}{ccc}
+\cos(q_{\theta}) & -\sin(q_{\theta}) & q_{x}\\
+\sin(q_{\theta}) & \cos(q_{\theta}) & q_{y}\\
+0 & 0 & 1
+\end{array}\right]\left[\begin{array}{ccc}
+1 & -\delta\theta & \delta x\\
+\delta\theta & 1 & \delta y\\
+0 & 0 & 1
+\end{array}\right]\right)=\left[\begin{array}{c}
+q_{x}+\cos(q_{\theta})\delta x-\sin(q_{\theta})\delta y\\
+q_{y}+\sin(q_{\theta})\delta x+\cos(q_{\theta})\delta y
+\end{array}\right]
+\]
+
+\end_inset
+
+which then explains the Jacobian 
+\begin_inset Formula $H$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Standard
+Lie groups are very relevant in the robotics context, and you can read more
+ here:
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf
+\end_layout
+
+\end_inset
+
+ 
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://github.com/borglab/gtsam/blob/develop/doc/math.pdf
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+In some cases you want to go even beyond Lie groups to a looser concept,
+ 
+\series bold
+manifolds
+\series default
+, because not all unknown variables behave like a group, e.g., the space of
+ 3D planes, 2D lines, directions in space, etc.
+ For manifolds we do not always have an exponential map, but we have a retractio
+n that plays the same role.
+ Some of this is explained here:
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://gtsam.org/notes/GTSAM-Concepts.html 
+\end_layout
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Subsection
@@ -1680,13 +1850,13 @@ UnaryFactor
 \series default
 \emph default
  instances, and add them to graph.
- GTSAM uses shared pointers to refer to factors in factor graphs, and 
+ GTSAM uses shared pointers to refer to factors, and 
 \series bold
 \emph on
-boost::make_shared
+emplace_shared
 \series default
 \emph default
- is a convenience function to simultaneously construct a class and create
+ is a convenience method to simultaneously construct a class and create
  a 
 \series bold
 \emph on
@@ -1694,22 +1864,6 @@ shared_ptr
 \series default
 \emph default
  to it.
- 
-\begin_inset Note Note
-status collapsed
-
-\begin_layout Plain Layout
-and on lines 4-6 we add three newly created 
-\series bold
-\emph on
-UnaryFactor
-\series default
-\emph default
- instances to the graph.
-\end_layout
-
-\end_inset
-
  We obtain the factor graph from Figure 
 \begin_inset CommandInset ref
 LatexCommand vref

examples/RangeISAMExample_plaza2.cpp

@@ -10,62 +10,81 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file RangeISAMExample_plaza1.cpp
+ * @file RangeISAMExample_plaza2.cpp
  * @brief A 2D Range SLAM example
  * @date June 20, 2013
- * @author FRank Dellaert
+ * @author Frank Dellaert
  */
 
-// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
+// Both relative poses and recovered trajectory poses will be stored as Pose2.
 #include <gtsam/geometry/Pose2.h>
+using gtsam::Pose2;
 
-// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized.
-// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+#include <gtsam/base/Vector.h>
-// Here we will use Symbols
+using gtsam::Vector;
+using gtsam::Vector3;
+
+// Unknown landmarks are of type Point2 (which is just a 2D Eigen vector).
+#include <gtsam/geometry/Point2.h>
+using gtsam::Point2;
+
+// Each variable in the system (poses and landmarks) must be identified with a
+// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
+// (X1, X2, L1). Here we will use Symbols.
 #include <gtsam/inference/Symbol.h>
+using gtsam::Symbol;
 
-// We want to use iSAM2 to solve the range-SLAM problem incrementally
+// We want to use iSAM2 to solve the range-SLAM problem incrementally.
 #include <gtsam/nonlinear/ISAM2.h>
 
-// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
+// iSAM2 requires as input a set set of new factors to be added stored in a
-// and initial guesses for any new variables used in the added factors
+// factor graph, and initial guesses for any new variables in the added factors.
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 
-// We will use a non-liear solver to batch-inituialize from the first 150 frames
+// We will use a non-linear solver to batch-initialize from the first 150 frames
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
-// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// Measurement functions are represented as 'factors'. Several common factors
-// have been provided with the library for solving robotics SLAM problems.
+// have been provided with the library for solving robotics SLAM problems:
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
 
-// Standard headers, added last, so we know headers above work on their own
+// Timing, with functions below, provides nice facilities to benchmark.
+#include <gtsam/base/timing.h>
+using gtsam::tictoc_print_;
+
+// Standard headers, added last, so we know headers above work on their own.
 #include <fstream>
 #include <iostream>
+#include <random>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
 
-using namespace std;
-using namespace gtsam;
 namespace NM = gtsam::noiseModel;
 
-// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+// Data is second UWB ranging dataset, B2 or "plaza 2", from
-// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+// "Navigating with Ranging Radios: Five Data Sets with Ground Truth"
+// by Joseph Djugash, Bradley Hamner, and Stephan Roth
+// https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf
 
 // load the odometry
 // DR: Odometry Input (delta distance traveled and delta heading change)
-//    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
+//    Time (sec)  Delta Distance Traveled (m) Delta Heading (rad)
-typedef pair<double, Pose2> TimedOdometry;
+using TimedOdometry = std::pair<double, Pose2>;
-list<TimedOdometry> readOdometry() {
+std::list<TimedOdometry> readOdometry() {
-  list<TimedOdometry> odometryList;
+  std::list<TimedOdometry> odometryList;
-  string data_file = findExampleDataFile("Plaza2_DR.txt");
+  std::string data_file = gtsam::findExampleDataFile("Plaza2_DR.txt");
-  ifstream is(data_file.c_str());
+  std::ifstream is(data_file.c_str());
 
   while (is) {
     double t, distance_traveled, delta_heading;
     is >> t >> distance_traveled >> delta_heading;
-    odometryList.push_back(
+    odometryList.emplace_back(t, Pose2(distance_traveled, 0, delta_heading));
-        TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
   }
   is.clear(); /* clears the end-of-file and error flags */
   return odometryList;
@@ -73,90 +92,85 @@ list<TimedOdometry> readOdometry() {
 
 // load the ranges from TD
 //    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
-typedef boost::tuple<double, size_t, double> RangeTriple;
+using RangeTriple = boost::tuple<double, size_t, double>;
-vector<RangeTriple> readTriples() {
+std::vector<RangeTriple> readTriples() {
-  vector<RangeTriple> triples;
+  std::vector<RangeTriple> triples;
-  string data_file = findExampleDataFile("Plaza2_TD.txt");
+  std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
-  ifstream is(data_file.c_str());
+  std::ifstream is(data_file.c_str());
 
   while (is) {
-    double t, sender, range;
+    double t, range, sender, receiver;
-    size_t receiver;
     is >> t >> sender >> receiver >> range;
-    triples.push_back(RangeTriple(t, receiver, range));
+    triples.emplace_back(t, receiver, range);
   }
   is.clear(); /* clears the end-of-file and error flags */
   return triples;
 }
 
 // main
-int main (int argc, char** argv) {
+int main(int argc, char** argv) {
-
   // load Plaza2 data
-  list<TimedOdometry> odometry = readOdometry();
+  std::list<TimedOdometry> odometry = readOdometry();
-//  size_t M = odometry.size();
+  size_t M = odometry.size();
+  std::cout << "Read " << M << " odometry entries." << std::endl;
 
-  vector<RangeTriple> triples = readTriples();
+  std::vector<RangeTriple> triples = readTriples();
   size_t K = triples.size();
+  std::cout << "Read " << K << " range triples." << std::endl;
 
   // parameters
-  size_t minK = 150; // minimum number of range measurements to process initially
+  size_t minK =
-  size_t incK = 25; // minimum number of range measurements to process after
+      150;  // minimum number of range measurements to process initially
-  bool groundTruth = false;
+  size_t incK = 25;  // minimum number of range measurements to process after
   bool robust = true;
 
   // Set Noise parameters
-  Vector priorSigmas = Vector3(1,1,M_PI);
+  Vector priorSigmas = Vector3(1, 1, M_PI);
   Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
-  double sigmaR = 100; // range standard deviation
+  double sigmaR = 100;        // range standard deviation
-  const NM::Base::shared_ptr // all same type
+  const NM::Base::shared_ptr  // all same type
-  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
+      priorNoise = NM::Diagonal::Sigmas(priorSigmas),  // prior
-  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
+      looseNoise = NM::Isotropic::Sigma(2, 1000),      // loose LM prior
-  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
+      odoNoise = NM::Diagonal::Sigmas(odoSigmas),      // odometry
-  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
+      gaussian = NM::Isotropic::Sigma(1, sigmaR),      // non-robust
-  rangeNoise = robust ? tukey : gaussian;
+      tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15),
+                                 gaussian),  // robust
+      rangeNoise = robust ? tukey : gaussian;
 
   // Initialize iSAM
-  ISAM2 isam;
+  gtsam::ISAM2 isam;
 
   // Add prior on first pose
-  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.021089);
-      M_PI - 2.02108900000000);
+  gtsam::NonlinearFactorGraph newFactors;
-  NonlinearFactorGraph newFactors;
   newFactors.addPrior(0, pose0, priorNoise);
-  Values initial;
+  gtsam::Values initial;
   initial.insert(0, pose0);
 
-  //  initialize points
+  // We will initialize landmarks randomly, and keep track of which landmarks we
-  if (groundTruth) { // from TL file
+  // already added with a set.
-    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
+  std::mt19937_64 rng;
-    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
+  std::normal_distribution<double> normal(0.0, 100.0);
-    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
+  std::set<Symbol> initializedLandmarks;
-    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
-  } else { // drawn from sigma=1 Gaussian in matlab version
-    initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
-    initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
-    initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
-    initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
-  }
 
   // set some loop variables
-  size_t i = 1; // step counter
+  size_t i = 1;  // step counter
-  size_t k = 0; // range measurement counter
+  size_t k = 0;  // range measurement counter
   bool initialized = false;
   Pose2 lastPose = pose0;
   size_t countK = 0;
 
   // Loop over odometry
   gttic_(iSAM);
-  for(const TimedOdometry& timedOdometry: odometry) {
+  for (const TimedOdometry& timedOdometry : odometry) {
-    //--------------------------------- odometry loop -----------------------------------------
+    //--------------------------------- odometry loop --------------------------
     double t;
     Pose2 odometry;
     boost::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
-    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry, odoNoise));
+    newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
+                                                           odoNoise);
 
     // predict pose and add as initial estimate
     Pose2 predictedPose = lastPose.compose(odometry);
@@ -166,17 +180,30 @@ int main (int argc, char** argv) {
     // Check if there are range factors to be added
     while (k < K && t >= boost::get<0>(triples[k])) {
       size_t j = boost::get<1>(triples[k]);
+      Symbol landmark_key('L', j);
       double range = boost::get<2>(triples[k]);
-      newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
+      newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
+          i, landmark_key, range, rangeNoise);
+      if (initializedLandmarks.count(landmark_key) == 0) {
+        std::cout << "adding landmark " << j << std::endl;
+        double x = normal(rng), y = normal(rng);
+        initial.insert(landmark_key, Point2(x, y));
+        initializedLandmarks.insert(landmark_key);
+        // We also add a very loose prior on the landmark in case there is only
+        // one sighting, which cannot fully determine the landmark.
+        newFactors.emplace_shared<gtsam::PriorFactor<Point2>>(
+            landmark_key, Point2(0, 0), looseNoise);
+      }
       k = k + 1;
       countK = countK + 1;
     }
 
     // Check whether to update iSAM 2
     if ((k > minK) && (countK > incK)) {
-      if (!initialized) { // Do a full optimize for first minK ranges
+      if (!initialized) {  // Do a full optimize for first minK ranges
+        std::cout << "Initializing at time " << k << std::endl;
         gttic_(batchInitialization);
-        LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
+        gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
         initial = batchOptimizer.optimize();
         gttoc_(batchInitialization);
         initialized = true;
@@ -185,21 +212,27 @@ int main (int argc, char** argv) {
       isam.update(newFactors, initial);
       gttoc_(update);
       gttic_(calculateEstimate);
-      Values result = isam.calculateEstimate();
+      gtsam::Values result = isam.calculateEstimate();
       gttoc_(calculateEstimate);
       lastPose = result.at<Pose2>(i);
-      newFactors = NonlinearFactorGraph();
+      newFactors = gtsam::NonlinearFactorGraph();
-      initial = Values();
+      initial = gtsam::Values();
       countK = 0;
     }
     i += 1;
-    //--------------------------------- odometry loop -----------------------------------------
+    //--------------------------------- odometry loop --------------------------
-  } // end for
+  }  // end for
   gttoc_(iSAM);
 
   // Print timings
   tictoc_print_();
 
+  // Print optimized landmarks:
+  gtsam::Values finalResult = isam.calculateEstimate();
+  for (auto&& landmark_key : initializedLandmarks) {
+    Point2 p = finalResult.at<Point2>(landmark_key);
+    std::cout << landmark_key << ":" << p.transpose() << "\n";
+  }
+
   exit(0);
 }
-

gtsam/discrete/AlgebraicDecisionTree.h

@@ -160,7 +160,7 @@ namespace gtsam {
               const typename Base::LabelFormatter& labelFormatter =
                   &DefaultFormatter) const {
       auto valueFormatter = [](const double& v) {
-        return (boost::format("%4.4g") % v).str();
+        return (boost::format("%4.8g") % v).str();
       };
       Base::print(s, labelFormatter, valueFormatter);
     }

gtsam/discrete/Assignment.h

@@ -33,6 +33,8 @@ namespace gtsam {
 template <class L>
 class Assignment : public std::map<L, size_t> {
  public:
+  using std::map<L, size_t>::operator=;
+
   void print(const std::string& s = "Assignment: ") const {
     std::cout << s << ": ";
     for (const typename Assignment::value_type& keyValue : *this)

gtsam/discrete/DecisionTree-inl.h

@@ -59,33 +59,41 @@ namespace gtsam {
     /** constant stored in this leaf */
     Y constant_;
 
-    /** Constructor from constant */
+    /** The number of assignments contained within this leaf.
-    Leaf(const Y& constant) :
+     * Particularly useful when leaves have been pruned.
-      constant_(constant) {}
+     */
+    size_t nrAssignments_;
 
-    /** return the constant */
+    /// Constructor from constant
+    Leaf(const Y& constant, size_t nrAssignments = 1)
+        : constant_(constant), nrAssignments_(nrAssignments) {}
+
+    /// Return the constant
     const Y& constant() const {
       return constant_;
     }
 
+    /// Return the number of assignments contained within this leaf.
+    size_t nrAssignments() const { return nrAssignments_; }
+
     /// Leaf-Leaf equality
     bool sameLeaf(const Leaf& q) const override {
       return constant_ == q.constant_;
     }
 
-    /// polymorphic equality: is q is a leaf, could be
+    /// polymorphic equality: is q a leaf and is it the same as this leaf?
     bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
-    /** equality up to tolerance */
+    /// equality up to tolerance
     bool equals(const Node& q, const CompareFunc& compare) const override {
       const Leaf* other = dynamic_cast<const Leaf*>(&q);
       if (!other) return false;
       return compare(this->constant_, other->constant_);
     }
 
-    /** print */
+    /// print
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
       std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
@@ -108,7 +116,14 @@ namespace gtsam {
 
     /** apply unary operator */
     NodePtr apply(const Unary& op) const override {
-      NodePtr f(new Leaf(op(constant_)));
+      NodePtr f(new Leaf(op(constant_), nrAssignments_));
+      return f;
+    }
+
+    /// Apply unary operator with assignment
+    NodePtr apply(const UnaryAssignment& op,
+                  const Assignment<L>& assignment) const override {
+      NodePtr f(new Leaf(op(assignment, constant_), nrAssignments_));
       return f;
     }
 
@@ -123,7 +138,8 @@ namespace gtsam {
 
     // Applying binary operator to two leaves results in a leaf
     NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
-      NodePtr h(new Leaf(op(fL.constant_, constant_)));  // fL op gL
+      // fL op gL
+      NodePtr h(new Leaf(op(fL.constant_, constant_), nrAssignments_));
       return h;
     }
 
@@ -134,7 +150,7 @@ namespace gtsam {
 
     /** choose a branch, create new memory ! */
     NodePtr choose(const L& label, size_t index) const override {
-      return NodePtr(new Leaf(constant()));
+      return NodePtr(new Leaf(constant(), nrAssignments()));
     }
 
     bool isLeaf() const override { return true; }
@@ -152,7 +168,10 @@ namespace gtsam {
     std::vector<NodePtr> branches_;
 
    private:
-    /** incremental allSame */
+    /**
+     * Incremental allSame.
+     * Records if all the branches are the same leaf.
+     */
     size_t allSame_;
 
     using ChoicePtr = boost::shared_ptr<const Choice>;
@@ -165,15 +184,22 @@ namespace gtsam {
 #endif
     }
 
-    /** If all branches of a choice node f are the same, just return a branch */
+    /// If all branches of a choice node f are the same, just return a branch.
     static NodePtr Unique(const ChoicePtr& f) {
-#ifndef DT_NO_PRUNING
+#ifndef GTSAM_DT_NO_PRUNING
       if (f->allSame_) {
         assert(f->branches().size() > 0);
         NodePtr f0 = f->branches_[0];
-        assert(f0->isLeaf());
+
+        size_t nrAssignments = 0;
+        for(auto branch: f->branches()) {
+          assert(branch->isLeaf());
+          nrAssignments +=
+              boost::dynamic_pointer_cast<const Leaf>(branch)->nrAssignments();
+        }
         NodePtr newLeaf(
-            new Leaf(boost::dynamic_pointer_cast<const Leaf>(f0)->constant()));
+            new Leaf(boost::dynamic_pointer_cast<const Leaf>(f0)->constant(),
+                     nrAssignments));
         return newLeaf;
       } else
 #endif
@@ -182,15 +208,13 @@ namespace gtsam {
 
     bool isLeaf() const override { return false; }
 
-    /** Constructor, given choice label and mandatory expected branch count */
+    /// Constructor, given choice label and mandatory expected branch count.
     Choice(const L& label, size_t count) :
       label_(label), allSame_(true) {
       branches_.reserve(count);
     }
 
-    /**
+    /// Construct from applying binary op to two Choice nodes.
-     * Construct from applying binary op to two Choice nodes
-     */
     Choice(const Choice& f, const Choice& g, const Binary& op) :
       allSame_(true) {
       // Choose what to do based on label
@@ -218,6 +242,7 @@ namespace gtsam {
       }
     }
 
+    /// Return the label of this choice node.
     const L& label() const {
       return label_;
     }
@@ -239,7 +264,7 @@ namespace gtsam {
       branches_.push_back(node);
     }
 
-    /** print (as a tree) */
+    /// print (as a tree).
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
       std::cout << s << " Choice(";
@@ -285,7 +310,7 @@ namespace gtsam {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
-    /** equality */
+    /// equality
     bool equals(const Node& q, const CompareFunc& compare) const override {
       const Choice* other = dynamic_cast<const Choice*>(&q);
       if (!other) return false;
@@ -298,7 +323,7 @@ namespace gtsam {
       return true;
     }
 
-    /** evaluate */
+    /// evaluate
     const Y& operator()(const Assignment<L>& x) const override {
 #ifndef NDEBUG
       typename Assignment<L>::const_iterator it = x.find(label_);
@@ -313,21 +338,57 @@ namespace gtsam {
       return (*child)(x);
     }
 
-    /**
+    /// Construct from applying unary op to a Choice node.
-     * Construct from applying unary op to a Choice node
-     */
     Choice(const L& label, const Choice& f, const Unary& op) :
       label_(label), allSame_(true) {
       branches_.reserve(f.branches_.size());  // reserve space
-      for (const NodePtr& branch : f.branches_) push_back(branch->apply(op));
+      for (const NodePtr& branch : f.branches_) {
+        push_back(branch->apply(op));
+      }
     }
 
-    /** apply unary operator */
+    /**
+     * @brief Constructor which accepts a UnaryAssignment op and the
+     * corresponding assignment.
+     *
+     * @param label The label for this node.
+     * @param f The original choice node to apply the op on.
+     * @param op Function to apply on the choice node. Takes Assignment and
+     * value as arguments.
+     * @param assignment The Assignment that will go to op.
+     */
+    Choice(const L& label, const Choice& f, const UnaryAssignment& op,
+           const Assignment<L>& assignment)
+        : label_(label), allSame_(true) {
+      branches_.reserve(f.branches_.size());  // reserve space
+
+      Assignment<L> assignment_ = assignment;
+
+      for (size_t i = 0; i < f.branches_.size(); i++) {
+        assignment_[label_] = i;  // Set assignment for label to i
+
+        const NodePtr branch = f.branches_[i];
+        push_back(branch->apply(op, assignment_));
+
+        // Remove the assignment so we are backtracking
+        auto assignment_it = assignment_.find(label_);
+        assignment_.erase(assignment_it);
+      }
+    }
+
+    /// apply unary operator.
     NodePtr apply(const Unary& op) const override {
       auto r = boost::make_shared<Choice>(label_, *this, op);
       return Unique(r);
     }
 
+    /// Apply unary operator with assignment
+    NodePtr apply(const UnaryAssignment& op,
+                  const Assignment<L>& assignment) const override {
+      auto r = boost::make_shared<Choice>(label_, *this, op, assignment);
+      return Unique(r);
+    }
+
     // Apply binary operator "h = f op g" on Choice node
     // Note op is not assumed commutative so we need to keep track of order
     // Simply calls apply on argument to call correct virtual method:
@@ -592,11 +653,13 @@ namespace gtsam {
       std::function<Y(const X&)> Y_of_X) const {
     using LY = DecisionTree<L, Y>;
 
-    // ugliness below because apparently we can't have templated virtual
+    // Ugliness below because apparently we can't have templated virtual
-    // functions If leaf, apply unary conversion "op" and create a unique leaf
+    // functions.
+    // If leaf, apply unary conversion "op" and create a unique leaf.
     using MXLeaf = typename DecisionTree<M, X>::Leaf;
-    if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f))
+    if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f)) {
-      return NodePtr(new Leaf(Y_of_X(leaf->constant())));
+      return NodePtr(new Leaf(Y_of_X(leaf->constant()), leaf->nrAssignments()));
+    }
 
     // Check if Choice
     using MXChoice = typename DecisionTree<M, X>::Choice;
@@ -617,7 +680,16 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // Functor performing depth-first visit without Assignment<L> argument.
+  /**
+   * Functor performing depth-first visit to each leaf with the leaf value as
+   * the argument.
+   *
+   * NOTE: We differentiate between leaves and assignments. Concretely, a 3
+   * binary variable tree will have 2^3=8 assignments, but based on pruning, it
+   * can have less than 8 leaves. For example, if a tree has all assignment
+   * values as 1, then pruning will cause the tree to have only 1 leaf yet 8
+   * assignments.
+   */
   template <typename L, typename Y>
   struct Visit {
     using F = std::function<void(const Y&)>;
@@ -646,28 +718,74 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // Functor performing depth-first visit with Assignment<L> argument.
+  /**
+   * Functor performing depth-first visit to each leaf with the Leaf object
+   * passed as an argument.
+   *
+   * NOTE: We differentiate between leaves and assignments. Concretely, a 3
+   * binary variable tree will have 2^3=8 assignments, but based on pruning, it
+   * can have <8 leaves. For example, if a tree has all assignment values as 1,
+   * then pruning will cause the tree to have only 1 leaf yet 8 assignments.
+   */
+  template <typename L, typename Y>
+  struct VisitLeaf {
+    using F = std::function<void(const typename DecisionTree<L, Y>::Leaf&)>;
+    explicit VisitLeaf(F f) : f(f) {}  ///< Construct from folding function.
+    F f;                           ///< folding function object.
+
+    /// Do a depth-first visit on the tree rooted at node.
+    void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
+      using Leaf = typename DecisionTree<L, Y>::Leaf;
+      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
+        return f(*leaf);
+
+      using Choice = typename DecisionTree<L, Y>::Choice;
+      auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      if (!choice)
+        throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr");
+      for (auto&& branch : choice->branches()) (*this)(branch);  // recurse!
+    }
+  };
+
+  template <typename L, typename Y>
+  template <typename Func>
+  void DecisionTree<L, Y>::visitLeaf(Func f) const {
+    VisitLeaf<L, Y> visit(f);
+    visit(root_);
+  }
+
+  /****************************************************************************/
+  /**
+   * Functor performing depth-first visit to each leaf with the leaf's
+   * `Assignment<L>` and value passed as arguments.
+   *
+   * NOTE: Follows the same pruning semantics as `visit`.
+   */
   template <typename L, typename Y>
   struct VisitWith {
-    using Choices = Assignment<L>;
+    using F = std::function<void(const Assignment<L>&, const Y&)>;
-    using F = std::function<void(const Choices&, const Y&)>;
     explicit VisitWith(F f) : f(f) {}  ///< Construct from folding function.
-    Choices choices;  ///< Assignment, mutating through recursion.
+    Assignment<L> assignment;  ///< Assignment, mutating through recursion.
-    F f;              ///< folding function object.
+    F f;                       ///< folding function object.
 
     /// Do a depth-first visit on the tree rooted at node.
     void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
       using Leaf = typename DecisionTree<L, Y>::Leaf;
       if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
-        return f(choices, leaf->constant());
+        return f(assignment, leaf->constant());
 
       using Choice = typename DecisionTree<L, Y>::Choice;
       auto choice = boost::dynamic_pointer_cast<const Choice>(node);
       if (!choice)
         throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
       for (size_t i = 0; i < choice->nrChoices(); i++) {
-        choices[choice->label()] = i;    // Set assignment for label to i
+        assignment[choice->label()] = i;  // Set assignment for label to i
+
         (*this)(choice->branches()[i]);  // recurse!
+
+        // Remove the choice so we are backtracking
+        auto choice_it = assignment.find(choice->label());
+        assignment.erase(choice_it);
       }
     }
   };
@@ -679,6 +797,14 @@ namespace gtsam {
     visit(root_);
   }
 
+  /****************************************************************************/
+  template <typename L, typename Y>
+  size_t DecisionTree<L, Y>::nrLeaves() const {
+    size_t total = 0;
+    visit([&total](const Y& node) { total += 1; });
+    return total;
+  }
+
   /****************************************************************************/
   // fold is just done with a visit
   template <typename L, typename Y>
@@ -689,12 +815,26 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // labels is just done with a visit
+  /**
+   * Get (partial) labels by performing a visit.
+   *
+   * This method performs a depth-first search to go to every leaf and records
+   * the keys assignment which leads to that leaf. Since the tree can be pruned,
+   * there might be a leaf at a lower depth which results in a partial
+   * assignment (i.e. not all keys are specified).
+   *
+   * E.g. given a tree with 3 keys, there may be a branch where the 3rd key has
+   * the same values for all the leaves. This leads to the branch being pruned
+   * so we get a leaf which is arrived at by just the first 2 keys and their
+   * assignments.
+   */
   template <typename L, typename Y>
   std::set<L> DecisionTree<L, Y>::labels() const {
     std::set<L> unique;
-    auto f = [&](const Assignment<L>& choices, const Y&) {
+    auto f = [&](const Assignment<L>& assignment, const Y&) {
-      for (auto&& kv : choices) unique.insert(kv.first);
+      for (auto&& kv : assignment) {
+        unique.insert(kv.first);
+      }
     };
     visitWith(f);
     return unique;
@@ -734,6 +874,19 @@ namespace gtsam {
     return DecisionTree(root_->apply(op));
   }
 
+  /// Apply unary operator with assignment
+  template <typename L, typename Y>
+  DecisionTree<L, Y> DecisionTree<L, Y>::apply(
+      const UnaryAssignment& op) const {
+    // It is unclear what should happen if tree is empty:
+    if (empty()) {
+      throw std::runtime_error(
+          "DecisionTree::apply(unary op) undefined for empty tree.");
+    }
+    Assignment<L> assignment;
+    return DecisionTree(root_->apply(op, assignment));
+  }
+
   /****************************************************************************/
   template<typename L, typename Y>
   DecisionTree<L, Y> DecisionTree<L, Y>::apply(const DecisionTree& g,

gtsam/discrete/DecisionTree.h

@@ -54,6 +54,7 @@ namespace gtsam {
 
     /** Handy typedefs for unary and binary function types */
     using Unary = std::function<Y(const Y&)>;
+    using UnaryAssignment = std::function<Y(const Assignment<L>&, const Y&)>;
     using Binary = std::function<Y(const Y&, const Y&)>;
 
     /** A label annotated with cardinality */
@@ -103,6 +104,8 @@ namespace gtsam {
                                                  &DefaultCompare) const = 0;
       virtual const Y& operator()(const Assignment<L>& x) const = 0;
       virtual Ptr apply(const Unary& op) const = 0;
+      virtual Ptr apply(const UnaryAssignment& op,
+                        const Assignment<L>& assignment) const = 0;
       virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
@@ -150,7 +153,7 @@ namespace gtsam {
     /** Create a constant */
     explicit DecisionTree(const Y& y);
 
-    /** Create a new leaf function splitting on a variable */
+    /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
     DecisionTree(const L& label, const Y& y1, const Y& y2);
 
     /** Allow Label+Cardinality for convenience */
@@ -216,9 +219,8 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /** Make virtual */
+    /// Make virtual
-    virtual ~DecisionTree() {
+    virtual ~DecisionTree() {}
-    }
 
     /// Check if tree is empty.
     bool empty() const { return !root_; }
@@ -232,9 +234,11 @@ namespace gtsam {
     /**
      * @brief Visit all leaves in depth-first fashion.
      *
-     * @param f side-effect taking a value.
+     * @param f (side-effect) Function taking a value.
      *
-     * @note Due to pruning, leaves might not exhaust choices.
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
      *
      * Example:
      *   int sum = 0;
@@ -247,18 +251,40 @@ namespace gtsam {
     /**
      * @brief Visit all leaves in depth-first fashion.
      *
-     * @param f side-effect taking an assignment and a value.
+     * @param f (side-effect) Function taking the leaf node pointer.
      *
-     * @note Due to pruning, leaves might not exhaust choices.
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
      *
      * Example:
      *   int sum = 0;
-     *   auto visitor = [&](const Assignment<L>& choices, int y) { sum += y; };
+     *   auto visitor = [&](int y) { sum += y; };
+     *   tree.visitWith(visitor);
+     */
+    template <typename Func>
+    void visitLeaf(Func f) const;
+
+    /**
+     * @brief Visit all leaves in depth-first fashion.
+     *
+     * @param f (side-effect) Function taking an assignment and a value.
+     *
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
+     *
+     * Example:
+     *   int sum = 0;
+     *   auto visitor = [&](const Assignment<L>& assignment, int y) { sum += y; };
      *   tree.visitWith(visitor);
      */
     template <typename Func>
     void visitWith(Func f) const;
 
+    /// Return the number of leaves in the tree.
+    size_t nrLeaves() const;
+
     /**
      * @brief Fold a binary function over the tree, returning accumulator.
      *
@@ -283,6 +309,16 @@ namespace gtsam {
     /** apply Unary operation "op" to f */
     DecisionTree apply(const Unary& op) const;
 
+    /**
+     * @brief Apply Unary operation "op" to f while also providing the
+     * corresponding assignment.
+     *
+     * @param op Function which takes Assignment<L> and Y as input and returns
+     * object of type Y.
+     * @return DecisionTree
+     */
+    DecisionTree apply(const UnaryAssignment& op) const;
+
     /** apply binary operation "op" to f and g */
     DecisionTree apply(const DecisionTree& g, const Binary& op) const;
 
@@ -337,6 +373,13 @@ namespace gtsam {
     return f.apply(op);
   }
 
+  /// Apply unary operator `op` with Assignment to DecisionTree `f`.
+  template<typename L, typename Y>
+  DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
+      const typename DecisionTree<L, Y>::UnaryAssignment& op) {
+    return f.apply(op);
+  }
+
   /// Apply binary operator `op` to DecisionTree `f`.
   template<typename L, typename Y>
   DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,

gtsam/discrete/DecisionTreeFactor.cpp

@@ -156,10 +156,7 @@ namespace gtsam {
   std::vector<std::pair<DiscreteValues, double>> DecisionTreeFactor::enumerate()
       const {
     // Get all possible assignments
-    std::vector<std::pair<Key, size_t>> pairs;
+    std::vector<std::pair<Key, size_t>> pairs = discreteKeys();
-    for (auto& key : keys()) {
-      pairs.emplace_back(key, cardinalities_.at(key));
-    }
     // Reverse to make cartesian product output a more natural ordering.
     std::vector<std::pair<Key, size_t>> rpairs(pairs.rbegin(), pairs.rend());
     const auto assignments = DiscreteValues::CartesianProduct(rpairs);
@@ -289,5 +286,43 @@ namespace gtsam {
         AlgebraicDecisionTree<Key>(keys, table),
         cardinalities_(keys.cardinalities()) {}
 
+  /* ************************************************************************ */
+  DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
+    const size_t N = maxNrAssignments;
+
+    // Get the probabilities in the decision tree so we can threshold.
+    std::vector<double> probabilities;
+    this->visitLeaf([&](const Leaf& leaf) {
+      size_t nrAssignments = leaf.nrAssignments();
+      double prob = leaf.constant();
+      probabilities.insert(probabilities.end(), nrAssignments, prob);
+    });
+
+    // The number of probabilities can be lower than max_leaves
+    if (probabilities.size() <= N) {
+      return *this;
+    }
+
+    std::sort(probabilities.begin(), probabilities.end(),
+              std::greater<double>{});
+
+    double threshold = probabilities[N - 1];
+
+    // Now threshold the decision tree
+    size_t total = 0;
+    auto thresholdFunc = [threshold, &total, N](const double& value) {
+      if (value < threshold || total >= N) {
+        return 0.0;
+      } else {
+        total += 1;
+        return value;
+      }
+    };
+    DecisionTree<Key, double> thresholded(*this, thresholdFunc);
+
+    // Create pruned decision tree factor and return.
+    return DecisionTreeFactor(this->discreteKeys(), thresholded);
+  }
+
   /* ************************************************************************ */
 }  // namespace gtsam

gtsam/discrete/DecisionTreeFactor.h

@@ -170,6 +170,26 @@ namespace gtsam {
     /// Return all the discrete keys associated with this factor.
     DiscreteKeys discreteKeys() const;
 
+    /**
+     * @brief Prune the decision tree of discrete variables.
+     *
+     * Pruning will set the leaves to be "pruned" to 0 indicating a 0
+     * probability. An assignment is pruned if it is not in the top
+     * `maxNrAssignments` values.
+     *
+     * A violation can occur if there are more
+     * duplicate values than `maxNrAssignments`. A violation here is the need to
+     * un-prune the decision tree (e.g. all assignment values are 1.0). We could
+     * have another case where some subset of duplicates exist (e.g. for a tree
+     * with 8 assignments we have 1, 1, 1, 1, 0.8, 0.7, 0.6, 0.5), but this is
+     * not a violation since the for `maxNrAssignments=5` the top values are (1,
+     * 0.8).
+     *
+     * @param maxNrAssignments The maximum number of assignments to keep.
+     * @return DecisionTreeFactor
+     */
+    DecisionTreeFactor prune(size_t maxNrAssignments) const;
+
     /// @}
     /// @name Wrapper support
     /// @{

gtsam/discrete/DiscreteLookupDAG.h

@@ -46,7 +46,7 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
    * @brief Construct a new Discrete Lookup Table object
    *
    * @param nFrontals number of frontal variables
-   * @param keys a orted list of gtsam::Keys
+   * @param keys a sorted list of gtsam::Keys
    * @param potentials the algebraic decision tree with lookup values
    */
   DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/discrete/DiscreteKey.h>  // make sure we have traits
 #include <gtsam/discrete/DiscreteValues.h>
 // headers first to make sure no missing headers
-//#define DT_NO_PRUNING
+//#define GTSAM_DT_NO_PRUNING
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DecisionTree-inl.h>  // for convert only
 #define DISABLE_TIMING

gtsam/discrete/tests/testDecisionTree.cpp

@@ -18,7 +18,7 @@
  */
 
 // #define DT_DEBUG_MEMORY
-// #define DT_NO_PRUNING
+// #define GTSAM_DT_NO_PRUNING
 #define DISABLE_DOT
 #include <gtsam/discrete/DecisionTree-inl.h>
 
@@ -90,6 +90,7 @@ struct DT : public DecisionTree<string, int> {
     auto valueFormatter = [](const int& v) {
       return (boost::format("%d") % v).str();
     };
+    std::cout << s;
     Base::print("", keyFormatter, valueFormatter);
   }
   /// Equality method customized to int node type
@@ -322,6 +323,49 @@ TEST(DecisionTree, Containers) {
   StringContainerTree converted(stringIntTree, container_of_int);
 }
 
+/* ************************************************************************** */
+// Test nrAssignments.
+TEST(DecisionTree, NrAssignments) {
+  pair<string, size_t> A("A", 2), B("B", 2), C("C", 2);
+  DT tree({A, B, C}, "1 1 1 1 1 1 1 1");
+  EXPECT(tree.root_->isLeaf());
+  auto leaf = boost::dynamic_pointer_cast<const DT::Leaf>(tree.root_);
+  EXPECT_LONGS_EQUAL(8, leaf->nrAssignments());
+
+  DT tree2({C, B, A}, "1 1 1 2 3 4 5 5");
+  /* The tree is
+    Choice(C) 
+    0 Choice(B) 
+    0 0 Leaf 1
+    0 1 Choice(A) 
+    0 1 0 Leaf 1
+    0 1 1 Leaf 2
+    1 Choice(B) 
+    1 0 Choice(A) 
+    1 0 0 Leaf 3
+    1 0 1 Leaf 4
+    1 1 Leaf 5
+  */
+
+  auto root = boost::dynamic_pointer_cast<const DT::Choice>(tree2.root_);
+  CHECK(root);
+  auto choice0 = boost::dynamic_pointer_cast<const DT::Choice>(root->branches()[0]);
+  CHECK(choice0);
+  EXPECT(choice0->branches()[0]->isLeaf());
+  auto choice00 = boost::dynamic_pointer_cast<const DT::Leaf>(choice0->branches()[0]);
+  CHECK(choice00);
+  EXPECT_LONGS_EQUAL(2, choice00->nrAssignments());
+
+  auto choice1 = boost::dynamic_pointer_cast<const DT::Choice>(root->branches()[1]);
+  CHECK(choice1);
+  auto choice10 = boost::dynamic_pointer_cast<const DT::Choice>(choice1->branches()[0]);
+  CHECK(choice10);
+  auto choice11 = boost::dynamic_pointer_cast<const DT::Leaf>(choice1->branches()[1]);
+  CHECK(choice11);
+  EXPECT(choice11->isLeaf());
+  EXPECT_LONGS_EQUAL(2, choice11->nrAssignments());
+}
+
 /* ************************************************************************** */
 // Test visit.
 TEST(DecisionTree, visit) {
@@ -346,6 +390,44 @@ TEST(DecisionTree, visitWith) {
   EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
 }
 
+/* ************************************************************************** */
+// Test visit, with Choices argument.
+TEST(DecisionTree, VisitWithPruned) {
+  // Create pruned tree
+  std::pair<string, size_t> A("A", 2), B("B", 2), C("C", 2);
+  std::vector<std::pair<string, size_t>> labels = {C, B, A};
+  std::vector<int> nodes = {0, 0, 2, 3, 4, 4, 6, 7};
+  DT tree(labels, nodes);
+
+  std::vector<Assignment<string>> choices;
+  auto func = [&](const Assignment<string>& choice, const int& d) {
+    choices.push_back(choice);
+  };
+  tree.visitWith(func);
+
+  EXPECT_LONGS_EQUAL(6, choices.size());
+
+  Assignment<string> expectedAssignment;
+
+  expectedAssignment = {{"B", 0}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(0));
+
+  expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(1));
+
+  expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(2));
+
+  expectedAssignment = {{"B", 0}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(3));
+
+  expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(4));
+
+  expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(5));
+}
+
 /* ************************************************************************** */
 // Test fold.
 TEST(DecisionTree, fold) {
@@ -413,6 +495,43 @@ TEST(DecisionTree, threshold) {
   EXPECT_LONGS_EQUAL(2, thresholded.fold(count, 0));
 }
 
+/* ************************************************************************** */
+// Test apply with assignment.
+TEST(DecisionTree, ApplyWithAssignment) {
+  // Create three level tree
+  vector<DT::LabelC> keys;
+  keys += DT::LabelC("C", 2), DT::LabelC("B", 2), DT::LabelC("A", 2);
+  DT tree(keys, "1 2 3 4 5 6 7 8");
+
+  DecisionTree<string, double> probTree(
+      keys, "0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08");
+  double threshold = 0.045;
+
+  // We test pruning one tree by indexing into another.
+  auto pruner = [&](const Assignment<string>& choices, const int& x) {
+    // Prune out all the leaves with even numbers
+    if (probTree(choices) < threshold) {
+      return 0;
+    } else {
+      return x;
+    }
+  };
+  DT prunedTree = tree.apply(pruner);
+
+  DT expectedTree(keys, "0 0 0 0 5 6 7 8");
+  EXPECT(assert_equal(expectedTree, prunedTree));
+
+  size_t count = 0;
+  auto counter = [&](const Assignment<string>& choices, const int& x) {
+    count += 1;
+    return x;
+  };
+  DT prunedTree2 = prunedTree.apply(counter);
+
+  // Check if apply doesn't enumerate all leaves.
+  EXPECT_LONGS_EQUAL(5, count);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/discrete/tests/testDecisionTreeFactor.cpp

@@ -106,6 +106,41 @@ TEST(DecisionTreeFactor, enumerate) {
   EXPECT(actual == expected);
 }
 
+/* ************************************************************************* */
+// Check pruning of the decision tree works as expected.
+TEST(DecisionTreeFactor, Prune) {
+  DiscreteKey A(1, 2), B(2, 2), C(3, 2);
+  DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
+
+  // Only keep the leaves with the top 5 values.
+  size_t maxNrAssignments = 5;
+  auto pruned5 = f.prune(maxNrAssignments);
+
+  // Pruned leaves should be 0
+  DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8");
+  EXPECT(assert_equal(expected, pruned5));
+
+  // Check for more extreme pruning where we only keep the top 2 leaves
+  maxNrAssignments = 2;
+  auto pruned2 = f.prune(maxNrAssignments);
+  DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
+  EXPECT(assert_equal(expected2, pruned2));
+
+  DiscreteKey D(4, 2);
+  DecisionTreeFactor factor(
+      D & C & B & A,
+      "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
+      "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
+
+  DecisionTreeFactor expected3(
+      D & C & B & A,
+      "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 "
+      "0.999952870000 1.0 1.0 1.0 1.0");
+  maxNrAssignments = 5;
+  auto pruned3 = factor.prune(maxNrAssignments);
+  EXPECT(assert_equal(expected3, pruned3));
+}
+
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, DotWithNames) {
   DiscreteKey A(12, 3), B(5, 2);
@@ -194,4 +229,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/geometry/Pose2.cpp

@@ -213,6 +213,14 @@ Point2 Pose2::transformTo(const Point2& point,
   return q;
 }
 
+Matrix Pose2::transformTo(const Matrix& points) const {
+  if (points.rows() != 2) {
+    throw std::invalid_argument("Pose2:transformTo expects 2*N matrix.");
+  }
+  const Matrix2 Rt = rotation().transpose();
+  return Rt * (points.colwise() - t_);  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transformFrom(const Point2& point,
@@ -224,6 +232,15 @@ Point2 Pose2::transformFrom(const Point2& point,
   return q + t_;
 }
 
+
+Matrix Pose2::transformFrom(const Matrix& points) const {
+  if (points.rows() != 2) {
+    throw std::invalid_argument("Pose2:transformFrom expects 2*N matrix.");
+  }
+  const Matrix2 R = rotation().matrix();
+  return (R * points).colwise() + t_;  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
     OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {
@@ -292,54 +309,77 @@ double Pose2::range(const Pose2& pose,
 }
 
 /* *************************************************************************
- * New explanation, from scan.ml
+ * Align finds the angle using a linear method:
- * It finds the angle using a linear method:
+ * a = Pose2::transformFrom(b) = t + R*b
- * q = Pose2::transformFrom(p) = t + R*p
  * We need to remove the centroids from the data to find the rotation
- * using dp=[dpx;dpy] and q=[dqx;dqy] we have
+ * using db=[dbx;dby] and a=[dax;day] we have
- *  |dqx|   |c  -s|     |dpx|     |dpx -dpy|     |c|
+ *  |dax|   |c  -s|     |dbx|     |dbx -dby|     |c|
  *  |   | = |     |  *  |   |  =  |        |  *  | | = H_i*cs
- *  |dqy|   |s   c|     |dpy|     |dpy  dpx|     |s|
+ *  |day|   |s   c|     |dby|     |dby  dbx|     |s|
  * where the Hi are the 2*2 matrices. Then we will minimize the criterion
- * J = \sum_i norm(q_i - H_i * cs)
+ * J = \sum_i norm(a_i - H_i * cs)
  * Taking the derivative with respect to cs and setting to zero we have
- * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
+ * cs = (\sum_i H_i' * a_i)/(\sum H_i'*H_i)
  * The hessian is diagonal and just divides by a constant, but this
  * normalization constant is irrelevant, since we take atan2.
- * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
+ * i.e., cos ~ sum(dbx*dax + dby*day) and sin ~ sum(-dby*dax + dbx*day)
  * The translation is then found from the centroids
- * as they also satisfy cq = t + R*cp, hence t = cq - R*cp
+ * as they also satisfy ca = t + R*cb, hence t = ca - R*cb
  */
 
-boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
+boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
-
+  const size_t n = ab_pairs.size();
-  size_t n = pairs.size();
+  if (n < 2) {
-  if (n<2) return boost::none; // we need at least two pairs
+    return boost::none;  // we need at least 2 pairs
+  }
 
   // calculate centroids
-  Point2 cp(0,0), cq(0,0);
+  Point2 ca(0, 0), cb(0, 0);
-  for(const Point2Pair& pair: pairs) {
+  for (const Point2Pair& pair : ab_pairs) {
-    cp += pair.first;
+    ca += pair.first;
-    cq += pair.second;
+    cb += pair.second;
   }
-  double f = 1.0/n;
+  const double f = 1.0/n;
-  cp *= f; cq *= f;
+  ca *= f;
+  cb *= f;
 
   // calculate cos and sin
-  double c=0,s=0;
+  double c = 0, s = 0;
-  for(const Point2Pair& pair: pairs) {
+  for (const Point2Pair& pair : ab_pairs) {
-    Point2 dp = pair.first - cp;
+    Point2 da = pair.first - ca;
-    Point2 dq = pair.second - cq;
+    Point2 db = pair.second - cb;
-    c += dp.x() * dq.x() + dp.y() * dq.y();
+    c += db.x() * da.x() + db.y() * da.y();
-    s += -dp.y() * dq.x() + dp.x() * dq.y();
+    s += -db.y() * da.x() + db.x() * da.y();
   }
 
   // calculate angle and translation
-  double theta = atan2(s,c);
+  const double theta = atan2(s, c);
-  Rot2 R = Rot2::fromAngle(theta);
+  const Rot2 R = Rot2::fromAngle(theta);
-  Point2 t = cq - R*cp;
+  const Point2 t = ca - R*cb;
   return Pose2(R, t);
 }
 
+boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
+  if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
+    throw std::invalid_argument(
+      "Pose2:Align expects 2*N matrices of equal shape.");
+  }
+  Point2Pairs ab_pairs;
+  for (size_t j=0; j < a.cols(); j++) {
+    ab_pairs.emplace_back(a.col(j), b.col(j));
+  }
+  return Pose2::Align(ab_pairs);
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+boost::optional<Pose2> align(const Point2Pairs& ba_pairs) {
+  Point2Pairs ab_pairs;
+  for (const Point2Pair &baPair : ba_pairs) {
+    ab_pairs.emplace_back(baPair.second, baPair.first);
+  }
+  return Pose2::Align(ab_pairs);
+}
+#endif
+
 /* ************************************************************************* */
 } // namespace gtsam

gtsam/geometry/Pose2.h

@@ -92,6 +92,18 @@ public:
     *this = Expmap(v);
   }
 
+  /**
+   *  Create Pose2 by aligning two point pairs
+   *  A pose aTb is estimated between pairs (a_point, b_point) such that 
+   *    a_point = aTb * b_point
+   *  Note this allows for noise on the points but in that case the mapping 
+   *  will not be exact.
+   */
+  static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
+
+  // Version of Pose2::Align that takes 2 matrices.
+  static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
+
   /// @}
   /// @name Testable
   /// @{
@@ -199,13 +211,29 @@ public:
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in world coordinates and transform to Pose.
+   * @param points 2*N matrix in world coordinates
+   * @return points in Pose coordinates, as 2*N Matrix
+   */
+  Matrix transformTo(const Matrix& points) const;
+
   /** Return point coordinates in global frame */
   GTSAM_EXPORT Point2 transformFrom(const Point2& point,
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in Pose coordinates and transform to world.
+   * @param points 2*N matrix in Pose coordinates
+   * @return points in world coordinates, as 2*N Matrix
+   */
+  Matrix transformFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
-  inline Point2 operator*(const Point2& point) const { return transformFrom(point);}
+  inline Point2 operator*(const Point2& point) const { 
+    return transformFrom(point);
+  }
 
   /// @}
   /// @name Standard Interface
@@ -315,12 +343,15 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
   return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 /**
+ * @deprecated Use static constructor (with reversed pairs!)
  * Calculate pose between a vector of 2D point correspondences (p,q)
  * where q = Pose2::transformFrom(p) = t + R*p
  */
-typedef std::pair<Point2,Point2> Point2Pair;
+GTSAM_EXPORT boost::optional<Pose2> 
-GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
+GTSAM_DEPRECATED align(const Point2Pairs& pairs);
+#endif
 
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};

gtsam/geometry/Pose3.cpp

@@ -59,7 +59,7 @@ Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
   Matrix6 adj;
-  adj << R, Z_3x3, A, R;
+  adj << R, Z_3x3, A, R;  // Gives [R 0; A R]
   return adj;
 }
 
@@ -354,6 +354,14 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
   return R_ * point + t_;
 }
 
+Matrix Pose3::transformFrom(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
+  }
+  const Matrix3 R = R_.matrix();
+  return (R * points).colwise() + t_;  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
     OptionalJacobian<3, 3> Hpoint) const {
@@ -374,6 +382,14 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
   return q;
 }
 
+Matrix Pose3::transformTo(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
+  }
+  const Matrix3 Rt = R_.transpose();
+  return Rt * (points.colwise() - t_);  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
                     OptionalJacobian<1, 3> Hpoint) const {
@@ -431,7 +447,7 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
 boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   const size_t n = abPointPairs.size();
   if (n < 3) {
-    return boost::none; // we need at least three pairs
+    return boost::none;  // we need at least three pairs
   }
 
   // calculate centroids
@@ -451,6 +467,19 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   return Pose3(aRb, aTb);
 }
 
+boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
+  if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
+    throw std::invalid_argument(
+      "Pose3:Align expects 3*N matrices of equal shape.");
+  }
+  Point3Pairs abPointPairs;
+  for (size_t j=0; j < a.cols(); j++) {
+    abPointPairs.emplace_back(a.col(j), b.col(j));
+  }
+  return Pose3::Align(abPointPairs);
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
   Point3Pairs abPointPairs;
   for (const Point3Pair &baPair : baPointPairs) {
@@ -458,6 +487,7 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
   }
   return Pose3::Align(abPointPairs);
 }
+#endif
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {

gtsam/geometry/Pose3.h

@@ -85,6 +85,9 @@ public:
    */
   static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
 
+  // Version of Pose3::Align that takes 2 matrices.
+  static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
+
   /// @}
   /// @name Testable
   /// @{
@@ -249,6 +252,13 @@ public:
   Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in Pose coordinates and transform to world.
+   * @param points 3*N matrix in Pose coordinates
+   * @return points in world coordinates, as 3*N Matrix
+   */
+  Matrix transformFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
   inline Point3 operator*(const Point3& point) const {
     return transformFrom(point);
@@ -264,6 +274,13 @@ public:
   Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in world coordinates and transform to Pose.
+   * @param points 3*N matrix in world coordinates
+   * @return points in Pose coordinates, as 3*N Matrix
+   */
+  Matrix transformTo(const Matrix& points) const;
+
   /// @}
   /// @name Standard Interface
   /// @{

gtsam/geometry/geometry.i

@@ -372,6 +372,9 @@ class Pose2 {
   Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
   Pose2(Vector v);
 
+  static boost::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
+  static boost::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Pose2& pose, double tol) const;
@@ -406,6 +409,10 @@ class Pose2 {
   gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
   gtsam::Point2 transformTo(const gtsam::Point2& p) const;
 
+  // Matrix versions
+  Matrix transformFrom(const Matrix& points) const;
+  Matrix transformTo(const Matrix& points) const;
+
   // Standard Interface
   double x() const;
   double y() const;
@@ -420,8 +427,6 @@ class Pose2 {
   void serialize() const;
 };
   
-boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);
-
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {
   // Standard Constructors
@@ -431,6 +436,9 @@ class Pose3 {
   Pose3(const gtsam::Pose2& pose2);
   Pose3(Matrix mat);
 
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Pose3& pose, double tol) const;
@@ -470,6 +478,10 @@ class Pose3 {
   gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
   gtsam::Point3 transformTo(const gtsam::Point3& point) const;
 
+  // Matrix versions
+  Matrix transformFrom(const Matrix& points) const;
+  Matrix transformTo(const Matrix& points) const;
+
   // Standard Interface
   gtsam::Rot3 rotation() const;
   gtsam::Point3 translation() const;

gtsam/geometry/tests/testPose2.cpp

@@ -717,81 +717,75 @@ TEST( Pose2, range_pose )
 /* ************************************************************************* */
 
 TEST(Pose2, align_1) {
-  Pose2 expected(Rot2::fromAngle(0), Point2(10,10));
+  Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
-
+  Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
-  vector<Point2Pair> correspondences;
+                        {Point2(30, 20), Point2(20, 10)}};
-  Point2Pair pq1(make_pair(Point2(0,0), Point2(10,10)));
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
-  Point2Pair pq2(make_pair(Point2(20,10), Point2(30,20)));
+  EXPECT(assert_equal(expected, *aTb));
-  correspondences += pq1, pq2;
-
-  boost::optional<Pose2> actual = align(correspondences);
-  EXPECT(assert_equal(expected, *actual));
 }
 
 TEST(Pose2, align_2) {
-  Point2 t(20,10);
+  Point2 t(20, 10);
   Rot2 R = Rot2::fromAngle(M_PI/2.0);
   Pose2 expected(R, t);
 
-  vector<Point2Pair> correspondences;
+  Point2 b1(0, 0), b2(10, 0);
-  Point2 p1(0,0), p2(10,0);
+  Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
-  Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2);
+                        {expected.transformFrom(b2), b2}};
-  EXPECT(assert_equal(Point2(20,10),q1));
-  EXPECT(assert_equal(Point2(20,20),q2));
-  Point2Pair pq1(make_pair(p1, q1));
-  Point2Pair pq2(make_pair(p2, q2));
-  correspondences += pq1, pq2;
 
-  boost::optional<Pose2> actual = align(correspondences);
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
-  EXPECT(assert_equal(expected, *actual));
+  EXPECT(assert_equal(expected, *aTb));
 }
 
 namespace align_3 {
-  Point2 t(10,10);
+  Point2 t(10, 10);
   Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
-  Point2 p1(0,0), p2(10,0), p3(10,10);
+  Point2 b1(0, 0), b2(10, 0), b3(10, 10);
-  Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3);
+  Point2 a1 = expected.transformFrom(b1),
+         a2 = expected.transformFrom(b2),
+         a3 = expected.transformFrom(b3);
 }
 
 TEST(Pose2, align_3) {
   using namespace align_3;
 
-  vector<Point2Pair> correspondences;
+  Point2Pairs ab_pairs;
-  Point2Pair pq1(make_pair(p1, q1));
+  Point2Pair ab1(make_pair(a1, b1));
-  Point2Pair pq2(make_pair(p2, q2));
+  Point2Pair ab2(make_pair(a2, b2));
-  Point2Pair pq3(make_pair(p3, q3));
+  Point2Pair ab3(make_pair(a3, b3));
-  correspondences += pq1, pq2, pq3;
+  ab_pairs += ab1, ab2, ab3;
 
-  boost::optional<Pose2> actual = align(correspondences);
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
-  EXPECT(assert_equal(expected, *actual));
+  EXPECT(assert_equal(expected, *aTb));
 }
 
 namespace {
   /* ************************************************************************* */
   // Prototype code to align two triangles using a rigid transform
   /* ************************************************************************* */
-  struct Triangle { size_t i_,j_,k_;};
+  struct Triangle { size_t i_, j_, k_;};
 
-  boost::optional<Pose2> align2(const Point2Vector& ps, const Point2Vector& qs,
+  boost::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
     const pair<Triangle, Triangle>& trianglePair) {
       const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
-      vector<Point2Pair> correspondences;
+      Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
-      correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]);
+                              {as[t1.j_], bs[t2.j_]},
-      return align(correspondences);
+                              {as[t1.k_], bs[t2.k_]}};
+      return Pose2::Align(ab_pairs);
   }
 }
 
 TEST(Pose2, align_4) {
   using namespace align_3;
 
-  Point2Vector ps,qs;
+  Point2Vector as, bs;
-  ps += p1, p2, p3;
+  as += a1, a2, a3;
-  qs += q3, q1, q2; // note in 3,1,2 order !
+  bs += b3, b1, b2;  // note in 3,1,2 order !
 
   Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
   Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
 
-  boost::optional<Pose2> actual = align2(ps, qs, make_pair(t1,t2));
+  boost::optional<Pose2> actual = align2(as, bs, {t1, t2});
   EXPECT(assert_equal(expected, *actual));
 }
 

gtsam/geometry/tests/testTriangulation.cpp

@@ -19,6 +19,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -38,7 +39,7 @@ using namespace boost::assign;
 // Some common constants
 
 static const boost::shared_ptr<Cal3_S2> sharedCal =  //
-    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
+    boost::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480);
 
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
@@ -95,11 +96,123 @@ TEST(triangulation, twoPoses) {
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
+//******************************************************************************
+// Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
+TEST(triangulation, twoPosesCal3DS2) {
+  static const boost::shared_ptr<Cal3DS2> sharedDistortedCal =  //
+      boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
+                                  -0.0003);
+
+  PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+
+  poses += pose1, pose2;
+  measurements += z1Distorted, z2Distorted;
+
+  double rank_tol = 1e-9;
+
+  // 1. Test simple DLT, perfect in no noise situation
+  bool optimize = false;
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, 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<Cal3DS2>(poses, sharedDistortedCal, 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)
+  measurements.at(0) += Point2(0.1, 0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+  optimize = false;
+  boost::optional<Point3> actual3 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
+
+  // 4. Now with optimization on
+  optimize = true;
+  boost::optional<Point3> actual4 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
+}
+
+//******************************************************************************
+// Simple test with a well-behaved two camera situation with Fisheye
+// calibration.
+TEST(triangulation, twoPosesFisheye) {
+  using Calibration = Cal3Fisheye;
+  static const boost::shared_ptr<Calibration> sharedDistortedCal =  //
+      boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
+                                      0.0001, -0.0003);
+
+  PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+
+  poses += pose1, pose2;
+  measurements += z1Distorted, z2Distorted;
+
+  double rank_tol = 1e-9;
+
+  // 1. Test simple DLT, perfect in no noise situation
+  bool optimize = false;
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, 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<Calibration>(poses, sharedDistortedCal, 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)
+  measurements.at(0) += Point2(0.1, 0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+  optimize = false;
+  boost::optional<Point3> actual3 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
+
+  // 4. Now with optimization on
+  optimize = true;
+  boost::optional<Point3> actual4 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
+}
+
 //******************************************************************************
 // Similar, but now with Bundler calibration
 TEST(triangulation, twoPosesBundler) {
   boost::shared_ptr<Cal3Bundler> bundlerCal =  //
-      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
+      boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
   PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
   PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
 
@@ -117,7 +230,8 @@ TEST(triangulation, twoPosesBundler) {
   double rank_tol = 1e-9;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3<Cal3Bundler>(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,8 +239,9 @@ TEST(triangulation, twoPosesBundler) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
-  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
+                                     optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
 }
 
 //******************************************************************************
@@ -336,6 +451,31 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 #endif
 }
 
+//******************************************************************************
+TEST(triangulation, fourPoses_distinct_Ks_distortion) {
+  Cal3DS2 K1(1500, 1200, 0, 640, 480,  -.3, 0.1, 0.0001, -0.0003);
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  PinholeCamera<Cal3DS2> camera1(pose1, K1);
+
+  // create second camera 1 meter to the right of first camera
+  Cal3DS2 K2(1600, 1300, 0, 650, 440,  -.2, 0.05, 0.0002, -0.0001);
+  PinholeCamera<Cal3DS2> camera2(pose2, K2);
+
+  // 1. Project two landmarks into two cameras and triangulate
+  Point2 z1 = camera1.project(landmark);
+  Point2 z2 = camera2.project(landmark);
+
+  CameraSet<PinholeCamera<Cal3DS2>> cameras;
+  Point2Vector measurements;
+
+  cameras += camera1, camera2;
+  measurements += z1, z2;
+
+  boost::optional<Point3> actual =  //
+          triangulatePoint3<Cal3DS2>(cameras, measurements);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+}
+
 //******************************************************************************
 TEST(triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);

gtsam/geometry/triangulation.h

@@ -227,6 +227,109 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFrom
   return projection_matrices;
 }
 
+/** Create a pinhole calibration from a different Cal3 object, removing
+ * distortion.
+ *
+ * @tparam CALIBRATION Original calibration object.
+ * @param cal Input calibration object.
+ * @return Cal3_S2 with only the pinhole elements of cal.
+ */
+template <class CALIBRATION>
+Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
+  const auto& K = cal.K();
+  return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
+}
+
+/** Internal undistortMeasurement to be used by undistortMeasurement and
+ * undistortMeasurements */
+template <class CALIBRATION, class MEASUREMENT>
+MEASUREMENT undistortMeasurementInternal(
+    const CALIBRATION& cal, const MEASUREMENT& measurement,
+    boost::optional<Cal3_S2> pinholeCal = boost::none) {
+  if (!pinholeCal) {
+    pinholeCal = createPinholeCalibration(cal);
+  }
+  return pinholeCal->uncalibrate(cal.calibrate(measurement));
+}
+
+/** Remove distortion for measurements so as if the measurements came from a
+ * pinhole camera.
+ *
+ * Removes distortion but maintains the K matrix of the initial cal. Operates by
+ * calibrating using full calibration and uncalibrating with only the pinhole
+ * component of the calibration.
+ * @tparam CALIBRATION Calibration type to use.
+ * @param cal Calibration with which measurements were taken.
+ * @param measurements Vector of measurements to undistort.
+ * @return measurements with the effect of the distortion of sharedCal removed.
+ */
+template <class CALIBRATION>
+Point2Vector undistortMeasurements(const CALIBRATION& cal,
+                                   const Point2Vector& measurements) {
+  Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
+  Point2Vector undistortedMeasurements;
+  // Calibrate with cal and uncalibrate with pinhole version of cal so that
+  // measurements are undistorted.
+  std::transform(measurements.begin(), measurements.end(),
+                 std::back_inserter(undistortedMeasurements),
+                 [&cal, &pinholeCalibration](const Point2& measurement) {
+                   return undistortMeasurementInternal<CALIBRATION>(
+                       cal, measurement, pinholeCalibration);
+                 });
+  return undistortedMeasurements;
+}
+
+/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
+template <>
+inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
+                                          const Point2Vector& measurements) {
+  return measurements;
+}
+
+/** Remove distortion for measurements so as if the measurements came from a
+ * pinhole camera.
+ *
+ * Removes distortion but maintains the K matrix of the initial calibrations.
+ * Operates by calibrating using full calibration and uncalibrating with only
+ * the pinhole component of the calibration.
+ * @tparam CAMERA Camera type to use.
+ * @param cameras Cameras corresponding to each measurement.
+ * @param measurements Vector of measurements to undistort.
+ * @return measurements with the effect of the distortion of the camera removed.
+ */
+template <class CAMERA>
+typename CAMERA::MeasurementVector undistortMeasurements(
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements) {
+  const size_t num_meas = cameras.size();
+  assert(num_meas == measurements.size());
+  typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
+  for (size_t ii = 0; ii < num_meas; ++ii) {
+    // Calibrate with cal and uncalibrate with pinhole version of cal so that
+    // measurements are undistorted.
+    undistortedMeasurements[ii] =
+        undistortMeasurementInternal<typename CAMERA::CalibrationType>(
+            cameras[ii].calibration(), measurements[ii]);
+  }
+  return undistortedMeasurements;
+}
+
+/** Specialize for Cal3_S2 to do nothing. */
+template <class CAMERA = PinholeCamera<Cal3_S2>>
+inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
+    const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
+    const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
+  return measurements;
+}
+
+/** Specialize for SphericalCamera to do nothing. */
+template <class CAMERA = SphericalCamera>
+inline SphericalCamera::MeasurementVector undistortMeasurements(
+    const CameraSet<SphericalCamera>& cameras,
+    const SphericalCamera::MeasurementVector& measurements) {
+  return measurements;
+}
+
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. The function checks that the
@@ -253,8 +356,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // construct projection matrices from poses & calibration
   auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
 
+  // Undistort the measurements, leaving only the pinhole elements in effect.
+  auto undistortedMeasurements =
+      undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+
   // Triangulate linearly
-  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
+  Point3 point =
+      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
 
   // Then refine using non-linear optimization
   if (optimize)
@@ -300,7 +408,13 @@ Point3 triangulatePoint3(
 
   // construct projection matrices from poses & calibration
   auto projection_matrices = projectionMatricesFromCameras(cameras);
-  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
+
+  // Undistort the measurements, leaving only the pinhole elements in effect.
+  auto undistortedMeasurements =
+      undistortMeasurements<CAMERA>(cameras, measurements);
+
+  Point3 point =
+      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
 
   // The n refine using non-linear optimization
   if (optimize)

gtsam/inference/BayesTree.h

@@ -140,7 +140,7 @@ namespace gtsam {
       return nodes_.empty();
     }
 
-    /** return nodes */
+    /** Return nodes. Each node is a clique of variables obtained after elimination. */
     const Nodes& nodes() const { return nodes_; }
 
     /** Access node by variable */

gtsam/inference/ClusterTree-inst.h

@@ -15,6 +15,10 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
 
+#ifdef GTSAM_USE_TBB
+#include <mutex>
+#endif
+
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -120,12 +124,25 @@ struct EliminationData {
   size_t myIndexInParent;
   FastVector<sharedFactor> childFactors;
   boost::shared_ptr<BTNode> bayesTreeNode;
+#ifdef GTSAM_USE_TBB
+  boost::shared_ptr<std::mutex> writeLock;
+#endif
 
   EliminationData(EliminationData* _parentData, size_t nChildren) :
-      parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
+      parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>())
+#ifdef GTSAM_USE_TBB
+      , writeLock(boost::make_shared<std::mutex>())
+#endif
+    {
     if (parentData) {
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->lock();
+#endif
       myIndexInParent = parentData->childFactors.size();
       parentData->childFactors.push_back(sharedFactor());
+#ifdef GTSAM_USE_TBB
+      parentData->writeLock->unlock();
+#endif
     } else {
       myIndexInParent = 0;
     }
@@ -196,8 +213,15 @@ struct EliminationData {
         nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
 
       // Store remaining factor in parent's gathered factors
-      if (!eliminationResult.second->empty())
+      if (!eliminationResult.second->empty()) {
+#ifdef GTSAM_USE_TBB
+        myData.parentData->writeLock->lock();
+#endif
         myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
+#ifdef GTSAM_USE_TBB
+        myData.parentData->writeLock->unlock();
+#endif
+      }
     }
   };
 };

gtsam/inference/Ordering.cpp

@@ -25,11 +25,7 @@
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
 
 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
-#ifdef GTSAM_USE_SYSTEM_METIS
 #include <metis.h>
-#else
-#include <gtsam/3rdparty/metis/include/metis.h>
-#endif
 #endif
 
 using namespace std;

gtsam/linear/GaussianConditional.cpp

@@ -89,11 +89,20 @@ namespace gtsam {
 
   /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
-    cout << s << "  Conditional density ";
+    cout << s << " p(";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
-      cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
+      cout << (boost::format("%1%") % (formatter(*it))).str()
+           << (nrFrontals() > 1 ? " " : "");
     }
-    cout << endl;
+
+    if (nrParents()) {
+      cout << " |";
+      for (const_iterator it = beginParents(); it != endParents(); ++it) {
+        cout << " " << (boost::format("%1%") % (formatter(*it))).str();
+      }
+    }
+    cout << ")" << endl;
+
     cout << formatMatrixIndented("  R = ", R()) << endl;
     for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
       cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))

gtsam/linear/GaussianConditional.h

@@ -109,8 +109,9 @@ namespace gtsam {
     /// @{
 
     /** print */
-    void print(const std::string& = "GaussianConditional",
+    void print(
-      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+        const std::string& = "GaussianConditional",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /** equals function */
     bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;

gtsam/linear/LossFunctions.h

@@ -223,6 +223,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar &BOOST_SERIALIZATION_NVP(k_);
+    ar &BOOST_SERIALIZATION_NVP(ksquared_);
   }
 };
 
@@ -273,6 +274,7 @@ class GTSAM_EXPORT Welsch : public Base {
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar &BOOST_SERIALIZATION_NVP(c_);
+    ar &BOOST_SERIALIZATION_NVP(csquared_);
   }
 };
 

gtsam/linear/NoiseModel.cpp

@@ -134,7 +134,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
 
 /* ************************************************************************* */
 void Gaussian::print(const string& name) const {
-  gtsam::print(thisR(), name + "Gaussian");
+  gtsam::print(thisR(), name + "Gaussian ");
 }
 
 /* ************************************************************************* */
@@ -285,7 +285,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
 
 /* ************************************************************************* */
 void Diagonal::print(const string& name) const {
-  gtsam::print(sigmas_, name + "diagonal sigmas");
+  gtsam::print(sigmas_, name + "diagonal sigmas ");
 }
 
 /* ************************************************************************* */
@@ -355,8 +355,8 @@ bool Constrained::constrained(size_t i) const {
 
 /* ************************************************************************* */
 void Constrained::print(const std::string& name) const {
-  gtsam::print(sigmas_, name + "constrained sigmas");
+  gtsam::print(sigmas_, name + "constrained sigmas ");
-  gtsam::print(mu_, name + "constrained mu");
+  gtsam::print(mu_, name + "constrained mu ");
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testGaussianConditional.cpp

@@ -40,6 +40,7 @@ using namespace gtsam;
 using namespace std;
 using namespace boost::assign;
 using symbol_shorthand::X;
+using symbol_shorthand::Y;
 
 static const double tol = 1e-5;
 
@@ -396,6 +397,55 @@ TEST(GaussianConditional, sample) {
   // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(GaussianConditional, Print) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
+  const Vector2 b(20, 40);
+  const double sigma = 3;
+
+  GaussianConditional conditional(X(0), b, Matrix2::Identity(),
+                                  noiseModel::Isotropic::Sigma(2, sigma));
+
+  // Test printing for no parents.
+  std::string expected =
+    "GaussianConditional p(x0)\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
+
+  auto conditional1 =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  // Test printing for single parent.
+  std::string expected1 =
+    "GaussianConditional p(x0 | x1)\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[x1] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
+
+  // Test printing for multiple parents.
+  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, Y(0), A2,
+                                                             Y(1), b, sigma);
+  std::string expected2 =
+    "GaussianConditional p(x0 | y0 y1)\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[y0] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  S[y1] = [ -5 -6 ]\n"
+    "          [ -7 -8 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/PreintegrationParams.cpp

@@ -34,7 +34,6 @@ void PreintegrationParams::print(const string& s) const {
        << endl;
   if (omegaCoriolis && use2ndOrderCoriolis)
     cout << "Using 2nd-order Coriolis" << endl;
-  if (body_P_sensor) body_P_sensor->print("    ");
   cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
 }
 

gtsam/nonlinear/ISAM2.h

@@ -295,6 +295,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
       const KeySet& relinKeys);
 
+  /**
+   * @brief Perform an incremental update of the factor graph to return a new
+   * Bayes Tree with affected keys.
+   *
+   * @param updateParams Parameters for the ISAM2 update.
+   * @param relinKeys Keys of variables to relinearize.
+   * @param affectedKeys The set of keys which are affected in the update.
+   * @param affectedKeysSet [output] Affected and contaminated keys.
+   * @param orphans [output] List of orphanes cliques after elimination.
+   * @param result [output] The result of the incremental update step.
+   */
   void recalculateIncremental(const ISAM2UpdateParams& updateParams,
                               const KeySet& relinKeys,
                               const FastList<Key>& affectedKeys,

gtsam/nonlinear/ISAM2Result.h

@@ -175,6 +175,7 @@ struct ISAM2Result {
   /** Getters and Setters */
   size_t getVariablesRelinearized() const { return variablesRelinearized; }
   size_t getVariablesReeliminated() const { return variablesReeliminated; }
+  FactorIndices getNewFactorsIndices() const { return newFactorsIndices; }
   size_t getCliques() const { return cliques; }
   double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
   double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }

gtsam/nonlinear/LevenbergMarquardtParams.h

@@ -35,7 +35,7 @@ class LevenbergMarquardtOptimizer;
 class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams {
 
 public:
-  /** See LevenbergMarquardtParams::lmVerbosity */
+  /** See LevenbergMarquardtParams::verbosityLM */
   enum VerbosityLM {
     SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
   };

gtsam/nonlinear/Values-inl.h

@@ -279,10 +279,11 @@ namespace gtsam {
    template <typename ValueType>
    struct handle {
      ValueType operator()(Key j, const Value* const pointer) {
-       try {
+       auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
+       if (ptr) {
          // value returns a const ValueType&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
+         return ptr->value();
-       } catch (std::bad_cast&) {
+       } else {
          throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
        }
      }
@@ -294,11 +295,12 @@ namespace gtsam {
    // Handle dynamic matrices
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
-     Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
+     inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
-       try {
+       auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
+       if (ptr) {
          // value returns a const Matrix&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
+         return ptr->value();
-       } catch (std::bad_cast&) {
+       } else {
          // If a fixed matrix was stored, we end up here as well.
          throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
        }
@@ -308,16 +310,18 @@ namespace gtsam {
    // Handle fixed matrices
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
-     Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
+     inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
-       try {
+       auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
+       if (ptr) {
          // value returns a const MatrixMN&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
+         return ptr->value();
-       } catch (std::bad_cast&) {
+       } else {
          Matrix A;
-         try {
+         // Check if a dynamic matrix was stored
-           // Check if a dynamic matrix was stored
+         auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
-           A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer);  // will throw if not....
+         if (ptr) {
-         } catch (const ValuesIncorrectType&) {
+           A = ptr->value();
+         } else {
            // Or a dynamic vector
            A = handle_matrix<Eigen::VectorXd, true>()(j, pointer);  // will throw if not....
          }
@@ -364,10 +368,10 @@ namespace gtsam {
 
     if(item != values_.end()) {
       // dynamic cast the type and throw exception if incorrect
-      const Value& value = *item->second;
+      auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
-      try {
+      if (ptr) {
-        return dynamic_cast<const GenericValue<ValueType>&>(value).value();
+        return ptr->value();
-      } catch (std::bad_cast &) {
+      } else {
         // NOTE(abe): clang warns about potential side effects if done in typeid
         const Value* value = item->second;
         throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));

gtsam/nonlinear/nonlinear.i

@@ -25,6 +25,7 @@ namespace gtsam {
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
+#include <gtsam/basis/ParameterMatrix.h>
 
 #include <gtsam/nonlinear/GraphvizFormatting.h>
 class GraphvizFormatting : gtsam::DotWriter {
@@ -228,6 +229,25 @@ class Values {
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
+  void insert(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
+
+  template <T = {gtsam::Point2,
+                 gtsam::Point3}>
+  void insert(size_t j, const T& val);
 
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
@@ -254,6 +274,21 @@ class Values {
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
   void update(size_t j, double c);
+  void update(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   void insert_or_assign(size_t j, const gtsam::Point2& point2);
   void insert_or_assign(size_t j, const gtsam::Point3& point3);
@@ -280,6 +315,21 @@ class Values {
   void insert_or_assign(size_t j, Vector vector);
   void insert_or_assign(size_t j, Matrix matrix);
   void insert_or_assign(size_t j, double c);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   template <T = {gtsam::Point2,
                  gtsam::Point3,
@@ -305,7 +355,22 @@ class Values {
                  gtsam::NavState,
                  Vector,
                  Matrix,
-                 double}>
+                 double,
+                 gtsam::ParameterMatrix<1>,
+                 gtsam::ParameterMatrix<2>,
+                 gtsam::ParameterMatrix<3>,
+                 gtsam::ParameterMatrix<4>,
+                 gtsam::ParameterMatrix<5>,
+                 gtsam::ParameterMatrix<6>,
+                 gtsam::ParameterMatrix<7>,
+                 gtsam::ParameterMatrix<8>,
+                 gtsam::ParameterMatrix<9>,
+                 gtsam::ParameterMatrix<10>,
+                 gtsam::ParameterMatrix<11>,
+                 gtsam::ParameterMatrix<12>,
+                 gtsam::ParameterMatrix<13>,
+                 gtsam::ParameterMatrix<14>,
+                 gtsam::ParameterMatrix<15>}>
   T at(size_t j);
 };
 
@@ -621,6 +686,7 @@ class ISAM2Result {
   /** Getters and Setters for all properties */
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
+  FactorIndices getNewFactorsIndices() const;
   size_t getCliques() const;
   double getErrorBefore() const;
   double getErrorAfter() const;

gtsam/sam/sam.i

@@ -20,6 +20,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  const double measured() const;
 };
 
 // between points:
@@ -54,6 +56,9 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  // Use `double` instead of template since that is all we need.
+  const double measured() const;
 };
 
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2>
@@ -73,6 +78,8 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  const BEARING& measured() const;
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2>

gtsam/sfm/SfmData.cpp

@@ -426,7 +426,6 @@ NonlinearFactorGraph SfmData::generalSfmFactors(
 NonlinearFactorGraph SfmData::sfmFactorGraph(
     const SharedNoiseModel &model, boost::optional<size_t> fixedCamera,
     boost::optional<size_t> fixedPoint) const {
-  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
   NonlinearFactorGraph graph = generalSfmFactors(model);
   using noiseModel::Constrained;
   if (fixedCamera) {

gtsam/slam/BetweenFactor.h

@@ -80,7 +80,9 @@ namespace gtsam {
     /// @{
 
     /// print with optional string
-    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    void print(
+        const std::string& s = "",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";

gtsam_unstable/gtsam_unstable.i

@@ -797,4 +797,30 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
 
+#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
+virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor {
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+      size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+    size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+        size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
+        bool verboseCheirality);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+      size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
+      bool verboseCheirality, gtsam::Pose3& body_P_sensor);
+
+  gtsam::Point2 measured() const;
+  double alpha() const;
+  gtsam::Cal3_S2* calibration() const;
+  bool verboseCheirality() const;
+  bool throwCheirality() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 } //\namespace gtsam

gtsam_unstable/partition/FindSeparator-inl.h

@@ -20,11 +20,10 @@
 
 #include "FindSeparator.h"
 
-#ifndef GTSAM_USE_SYSTEM_METIS
+#include <metis.h>
 
 extern "C" {
-#include <metis.h>
+#include <metislib.h>
-#include "metislib.h"
 }
 
 
@@ -566,5 +565,3 @@ namespace gtsam { namespace partition {
   }
 
 }} //namespace
-
-#endif

gtsam_unstable/partition/tests/testFindSeparator.cpp

@@ -20,8 +20,6 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::partition;
 
-#ifndef GTSAM_USE_SYSTEM_METIS
-
 /* ************************************************************************* */
 // x0 - x1 - x2
 // l3        l4
@@ -229,8 +227,6 @@ TEST ( Partition, findSeparator3_with_reduced_camera )
   LONGS_EQUAL(2, partitionTable[28]);
 }
 
-#endif
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam_unstable/slam/InvDepthFactor3.h

@@ -92,7 +92,7 @@ public:
     } catch( CheiralityException& e) {
       if (H1) *H1 = Matrix::Zero(2,6);
       if (H2) *H2 = Matrix::Zero(2,5);
-      if (H3) *H2 = Matrix::Zero(2,1);
+      if (H3) *H3 = Matrix::Zero(2,1);
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();

gtsam_unstable/slam/tests/testInvDepthFactor3.cpp

@@ -1,8 +1,18 @@
-/*
+/* ----------------------------------------------------------------------------
- * testInvDepthFactor.cpp
+ * 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  testInvDepthFactor3.cpp
+ *  @brief Unit tests inverse depth parametrization
  *
- *  Created on: Apr 13, 2012
+ *  @author cbeall3
- *      Author: cbeall3
+ *  @author Dominik Van Opdenbosch
+ *  @date   Apr 13, 2012
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -12,6 +22,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam_unstable/slam/InvDepthFactor3.h>
 
@@ -28,6 +39,11 @@ PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
 typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
 typedef NonlinearEquality<Pose3> PoseConstraint;
 
+Matrix factorError(const Pose3& pose, const Vector5& point, double invDepth,
+                     const InverseDepthFactor& factor) {
+  return factor.evaluateError(pose, point, invDepth);
+}
+
 /* ************************************************************************* */
 TEST( InvDepthFactor, optimize) {
 
@@ -92,6 +108,55 @@ TEST( InvDepthFactor, optimize) {
 }
 
 
+/* ************************************************************************* */
+TEST( InvDepthFactor, Jacobian3D ) {
+
+  // landmark 5 meters infront of camera (camera center at (0,0,1))
+  Point3 landmark(5, 0, 1);
+
+  // get expected projection using pinhole camera
+  Point2 expected_uv = level_camera.project(landmark);
+
+  // get expected landmark representation using backprojection
+  double inv_depth;
+  Vector5 inv_landmark;
+  InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
+  std::tie(inv_landmark, inv_depth) = inv_camera.backproject(expected_uv, 5);
+  Vector5 expected_inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
+
+  CHECK(assert_equal(expected_inv_landmark, inv_landmark, 1e-6));
+  CHECK(assert_equal(inv_depth, 1./5, 1e-6));
+
+  Symbol poseKey('x',1);
+  Symbol pointKey('l',1);
+  Symbol invDepthKey('d',1);
+  InverseDepthFactor factor(expected_uv, sigma, poseKey, pointKey, invDepthKey, K);
+
+  std::vector<Matrix> actualHs(3);
+  factor.unwhitenedError({{poseKey, genericValue(level_pose)},
+                          {pointKey, genericValue(inv_landmark)},
+                          {invDepthKey,genericValue(inv_depth)}},
+                         actualHs);
+
+  const Matrix& H1Actual = actualHs.at(0);
+  const Matrix& H2Actual = actualHs.at(1);
+  const Matrix& H3Actual = actualHs.at(2);
+
+  // Use numerical derivatives to verify the Jacobians
+  Matrix H1Expected, H2Expected, H3Expected;
+
+  std::function<Matrix(const Pose3 &, const Vector5 &, const double &)>
+      func = std::bind(&factorError, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, factor);
+  H1Expected = numericalDerivative31(func, level_pose, inv_landmark, inv_depth);
+  H2Expected = numericalDerivative32(func, level_pose, inv_landmark, inv_depth);
+  H3Expected = numericalDerivative33(func, level_pose, inv_landmark, inv_depth);
+
+  // Verify the Jacobians
+  CHECK(assert_equal(H1Expected, H1Actual, 1e-6))
+  CHECK(assert_equal(H2Expected, H2Actual, 1e-6))
+  CHECK(assert_equal(H3Expected, H3Actual, 1e-6))
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

matlab/CMakeLists.txt

@@ -92,8 +92,8 @@ if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
   endif()
 
   # Wrap
-  matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" ""
+  matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam_unstable"
-              "${mexFlags}" "${ignore}")
+              "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}")
 endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
 
 # Record the root dir for gtsam - needed during external builds, e.g., ROS

python/CMakeLists.txt

@@ -173,7 +173,7 @@ endif()
 # Add custom target so we can install with `make python-install`
 set(GTSAM_PYTHON_INSTALL_TARGET python-install)
 add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
-        COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install
+        COMMAND ${PYTHON_EXECUTABLE} -m pip install --user .
         DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
         WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
 

python/README.md

@@ -8,6 +8,7 @@ For instructions on updating the version of the [wrap library](https://github.co
 
 ## Requirements
 
+- Cmake >= 3.15
 - If you want to build the GTSAM python library for a specific python version (eg 3.6),
   use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
 - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),

python/gtsam/tests/test_GaussianFactorGraph.py

@@ -32,14 +32,14 @@ def create_graph():
     PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
 
     graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
-    graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
+    graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
     graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
 
     return graph, (x0, x1, x2)
 
+
 class TestGaussianFactorGraph(GtsamTestCase):
     """Tests for Gaussian Factor Graphs."""
-
     def test_fg(self):
         """Test solving a linear factor graph"""
         graph, X = create_graph()
@@ -88,5 +88,23 @@ class TestGaussianFactorGraph(GtsamTestCase):
         self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
         self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
 
+    def test_ordering(self):
+        """Test ordering"""
+        gfg, keys = create_graph()
+        ordering = gtsam.Ordering()
+        for key in keys[::-1]:
+            ordering.push_back(key)
+
+        bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
+
+        keyVector = gtsam.KeyVector()
+        keyVector.append(keys[2])
+        #TODO(Varun) Below code causes segfault in Debug config
+        # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)
+        # bn = gfg.eliminateSequential(ordering)
+        # self.assertEqual(bn.size(), 3)
+
+
 if __name__ == '__main__':
     unittest.main()

python/gtsam/tests/test_Pose2.py

@@ -8,11 +8,11 @@ See LICENSE for the license information
 Pose2 unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta & John Lambert
 """
+import math
 import unittest
 
-import numpy as np
-
 import gtsam
+import numpy as np
 from gtsam import Point2, Point2Pairs, Pose2
 from gtsam.utils.test_case import GtsamTestCase
 
@@ -26,6 +26,34 @@ class TestPose2(GtsamTestCase):
         actual = Pose2.adjoint_(xi, xi)
         np.testing.assert_array_equal(actual, expected)
 
+    def test_transformTo(self):
+        """Test transformTo method."""
+        pose = Pose2(2, 4, -math.pi/2)
+        actual = pose.transformTo(Point2(3, 2))
+        expected = Point2(2, 1)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        # multi-point version
+        points = np.stack([Point2(3, 2), Point2(3, 2)]).T
+        actual_array = pose.transformTo(points)
+        self.assertEqual(actual_array.shape, (2, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
+
+    def test_transformFrom(self):
+        """Test transformFrom method."""
+        pose = Pose2(2, 4, -math.pi/2)
+        actual = pose.transformFrom(Point2(2, 1))
+        expected = Point2(3, 2)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        # multi-point version
+        points = np.stack([Point2(2, 1), Point2(2, 1)]).T
+        actual_array = pose.transformFrom(points)
+        self.assertEqual(actual_array.shape, (2, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
+
     def test_align(self) -> None:
         """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
 
@@ -42,27 +70,36 @@ class TestPose2(GtsamTestCase):
               O---O
         """
         pts_a = [
-            Point2(3, 1),
-            Point2(1, 1),
-            Point2(1, 3),
-            Point2(3, 3),
-        ]
-        pts_b = [
             Point2(1, -3),
             Point2(1, -5),
             Point2(-1, -5),
             Point2(-1, -3),
         ]
+        pts_b = [
+            Point2(3, 1),
+            Point2(1, 1),
+            Point2(1, 3),
+            Point2(3, 3),
+        ]
 
         # fmt: on
         ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
-        bTa = gtsam.align(ab_pairs)
+        aTb = Pose2.Align(ab_pairs)
-        aTb = bTa.inverse()
+        self.assertIsNotNone(aTb)
-        assert aTb is not None
 
         for pt_a, pt_b in zip(pts_a, pts_b):
             pt_a_ = aTb.transformFrom(pt_b)
-            assert np.allclose(pt_a, pt_a_)
+            np.testing.assert_allclose(pt_a, pt_a_)
+
+        # Matrix version
+        A = np.array(pts_a).T
+        B = np.array(pts_b).T
+        aTb = Pose2.Align(A, B)
+        self.assertIsNotNone(aTb)
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            np.testing.assert_allclose(pt_a, pt_a_)
 
 
 if __name__ == "__main__":

python/gtsam/tests/test_Pose3.py

@@ -15,7 +15,7 @@ import unittest
 import numpy as np
 
 import gtsam
-from gtsam import Point3, Pose3, Rot3
+from gtsam import Point3, Pose3, Rot3, Point3Pairs
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -30,13 +30,34 @@ class TestPose3(GtsamTestCase):
         actual = T2.between(T3)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
-    def test_transform_to(self):
+    def test_transformTo(self):
         """Test transformTo method."""
-        transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
+        pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
-        actual = transform.transformTo(Point3(3, 2, 10))
+        actual = pose.transformTo(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        # multi-point version
+        points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T
+        actual_array = pose.transformTo(points)
+        self.assertEqual(actual_array.shape, (3, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
+
+    def test_transformFrom(self):
+        """Test transformFrom method."""
+        pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        actual = pose.transformFrom(Point3(2, 1, 10))
+        expected = Point3(3, 2, 10)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        # multi-point version
+        points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
+        actual_array = pose.transformFrom(points)
+        self.assertEqual(actual_array.shape, (3, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
+
     def test_range(self):
         """Test range method."""
         l1 = Point3(1, 0, 0)
@@ -81,6 +102,24 @@ class TestPose3(GtsamTestCase):
         actual.deserialize(serialized)
         self.gtsamAssertEquals(expected, actual, 1e-10)
 
+    def test_align_squares(self):
+        """Test if Align method can align 2 squares."""
+        square = np.array([[0,0,0],[0,1,0],[1,1,0],[1,0,0]], float).T
+        sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
+        transformed = sTt.transformTo(square)
+
+        st_pairs = Point3Pairs()
+        for j in range(4):
+            st_pairs.append((square[:,j], transformed[:,j]))
+
+        # Recover the transformation sTt
+        estimated_sTt = Pose3.Align(st_pairs)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
+        # Matrix version
+        estimated_sTt = Pose3.Align(square, transformed)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
 
 if __name__ == "__main__":
     unittest.main()

python/gtsam/tests/test_VisualISAMExample.py

@@ -10,9 +10,6 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import unittest
 
-import numpy as np
-
-import gtsam
 import gtsam.utils.visual_data_generator as generator
 import gtsam.utils.visual_isam as visual_isam
 from gtsam import symbol
@@ -20,8 +17,9 @@ from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestVisualISAMExample(GtsamTestCase):
-
+    """Test class for ISAM2 with visual landmarks."""
     def test_VisualISAMExample(self):
+        """Test to see if ISAM works as expected for a simple visual SLAM example."""
         # Data Options
         options = generator.Options()
         options.triangle = False
@@ -39,19 +37,22 @@ class TestVisualISAMExample(GtsamTestCase):
         data, truth = generator.generate_data(options)
 
         # Initialize iSAM with the first pose and points
-        isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
+        isam, result, nextPose = visual_isam.initialize(
+            data, truth, isamOptions)
 
         # Main loop for iSAM: stepping through all poses
         for currentPose in range(nextPose, options.nrCameras):
-            isam, result = visual_isam.step(data, isam, result, truth, currentPose)
+            isam, result = visual_isam.step(data, isam, result, truth,
+                                            currentPose)
 
-        for i in range(len(truth.cameras)):
+        for i, _ in enumerate(truth.cameras):
             pose_i = result.atPose3(symbol('x', i))
             self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
-        for j in range(len(truth.points)):
+        for j, _ in enumerate(truth.points):
             point_j = result.atPoint3(symbol('l', j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
+
 if __name__ == "__main__":
     unittest.main()

python/gtsam/tests/test_sam.py

@@ -0,0 +1,55 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Basis unit tests.
+Author: Frank Dellaert & Varun Agrawal (Python)
+"""
+import unittest
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSam(GtsamTestCase):
+    """
+    Tests python binding for classes/functions in `sam.i`.
+    """
+    def test_RangeFactor2D(self):
+        """
+        Test that `measured` works as expected for RangeFactor2D.
+        """
+        measurement = 10.0
+        factor = gtsam.RangeFactor2D(1, 2, measurement,
+                                     gtsam.noiseModel.Isotropic.Sigma(1, 1))
+        self.assertEqual(measurement, factor.measured())
+
+    def test_BearingFactor2D(self):
+        """
+        Test that `measured` works as expected for BearingFactor2D.
+        """
+        measurement = gtsam.Rot2(.3)
+        factor = gtsam.BearingFactor2D(1, 2, measurement,
+                                       gtsam.noiseModel.Isotropic.Sigma(1, 1))
+        self.gtsamAssertEquals(measurement, factor.measured())
+
+    def test_BearingRangeFactor2D(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactor2D.
+        """
+        range_measurement = 10.0
+        bearing_measurement = gtsam.Rot2(0.3)
+        factor = gtsam.BearingRangeFactor2D(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(2, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+ProjectionFactorRollingShutter unit tests.
+Author: Yotam Stern
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+import gtsam_unstable
+from gtsam.utils.test_case import GtsamTestCase
+
+
+pose1 = gtsam.Pose3()
+pose2 = gtsam.Pose3(np.array([[ 0.9999375 ,  0.00502487,  0.00998725,  0.1       ],
+                              [-0.00497488,  0.999975  , -0.00502487,  0.02      ],
+                              [-0.01001225,  0.00497488,  0.9999375 ,  1.        ],
+                              [ 0.        ,  0.        ,  0.        ,  1.        ]]))
+point = np.array([2, 0, 15])
+point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2))
+cal = gtsam.Cal3_S2()
+body_p_sensor = gtsam.Pose3()
+alpha = 0.1
+measured = np.array([0.13257015, 0.0004157])
+
+
+class TestProjectionFactorRollingShutter(GtsamTestCase):
+
+    def test_constructor(self):
+        '''
+        test constructor for the ProjectionFactorRollingShutter
+        '''
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal,
+                                                               body_p_sensor)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False,
+                                                               body_p_sensor)
+
+    def test_error(self):
+        '''
+        test the factor error for a specific example
+        '''
+        values = gtsam.Values()
+        values.insert(0, pose1)
+        values.insert(1, pose2)
+        values.insert(2, point)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
+        self.gtsamAssertEquals(factor.error(values), np.array(0), tol=1e-9)
+
+
+if __name__ == '__main__':
+    unittest.main()

wrap/gtwrap/pybind_wrapper.py

@@ -412,7 +412,7 @@ class PybindWrapper:
 
     def wrap_instantiated_declaration(
             self, instantiated_decl: instantiator.InstantiatedDeclaration):
-        """Wrap the class."""
+        """Wrap the forward declaration."""
         module_var = self._gen_module_var(instantiated_decl.namespaces())
         cpp_class = instantiated_decl.to_cpp()
         if cpp_class in self.ignore_classes:
@@ -420,7 +420,7 @@ class PybindWrapper:
 
         res = (
             '\n    py::class_<{cpp_class}, '
-            '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
+            '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}");'
         ).format(shared_ptr_type=('boost' if self.use_boost else 'std'),
                  cpp_class=cpp_class,
                  class_name=instantiated_decl.name,

wrap/matlab.h

@@ -37,15 +37,16 @@ extern "C" {
 #include <mex.h>
 }
 
-#include <boost/shared_ptr.hpp>
+#include <boost/cstdint.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
 
 #include <list>
-#include <string>
-#include <sstream>
-#include <typeinfo>
 #include <set>
+#include <sstream>
 #include <streambuf>
+#include <string>
+#include <typeinfo>
 
 using namespace std;
 using namespace boost; // not usual, but for conciseness of generated code

wrap/pybind11/.appveyor.yml

@@ -19,7 +19,7 @@ install:
     if ($env:PLATFORM -eq "x64") { $env:PYTHON = "$env:PYTHON-x64" }
     $env:PATH = "C:\Python$env:PYTHON\;C:\Python$env:PYTHON\Scripts\;$env:PATH"
     python -W ignore -m pip install --upgrade pip wheel
-    python -W ignore -m pip install pytest numpy --no-warn-script-location
+    python -W ignore -m pip install pytest numpy --no-warn-script-location pytest-timeout
 - ps: |
     Start-FileDownload 'https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip'
     7z x eigen-3.3.7.zip -y > $null

wrap/pybind11/.clang-format

@@ -0,0 +1,19 @@
+---
+# See all possible options and defaults with:
+# clang-format --style=llvm --dump-config
+BasedOnStyle: LLVM
+AccessModifierOffset: -4
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBinaryOperators: All
+BreakConstructorInitializers: BeforeColon
+ColumnLimit: 99
+IndentCaseLabels: true
+IndentPPDirectives: AfterHash
+IndentWidth: 4
+Language: Cpp
+SpaceAfterCStyleCast: true
+Standard: Cpp11
+TabWidth: 4
+...

wrap/pybind11/.clang-tidy

@@ -1,13 +1,66 @@
 FormatStyle: file
 
 Checks: '
+*bugprone*,
+cppcoreguidelines-init-variables,
+cppcoreguidelines-slicing,
+clang-analyzer-optin.cplusplus.VirtualCall,
+google-explicit-constructor,
 llvm-namespace-comment,
-modernize-use-override,
+misc-misplaced-const,
-readability-container-size-empty,
+misc-non-copyable-objects,
-modernize-use-using,
+misc-static-assert,
-modernize-use-equals-default,
+misc-throw-by-value-catch-by-reference,
+misc-uniqueptr-reset-release,
+misc-unused-parameters,
+modernize-avoid-bind,
+modernize-make-shared,
+modernize-redundant-void-arg,
+modernize-replace-auto-ptr,
+modernize-replace-disallow-copy-and-assign-macro,
+modernize-replace-random-shuffle,
+modernize-shrink-to-fit,
 modernize-use-auto,
+modernize-use-bool-literals,
+modernize-use-equals-default,
+modernize-use-equals-delete,
+modernize-use-default-member-init,
+modernize-use-noexcept,
 modernize-use-emplace,
+modernize-use-override,
+modernize-use-using,
+*performance*,
+readability-avoid-const-params-in-decls,
+readability-const-return-type,
+readability-container-size-empty,
+readability-delete-null-pointer,
+readability-else-after-return,
+readability-implicit-bool-conversion,
+readability-make-member-function-const,
+readability-misplaced-array-index,
+readability-non-const-parameter,
+readability-redundant-function-ptr-dereference,
+readability-redundant-smartptr-get,
+readability-redundant-string-cstr,
+readability-simplify-subscript-expr,
+readability-static-accessed-through-instance,
+readability-static-definition-in-anonymous-namespace,
+readability-string-compare,
+readability-suspicious-call-argument,
+readability-uniqueptr-delete-release,
+-bugprone-exception-escape,
+-bugprone-reserved-identifier,
+-bugprone-unused-raii,
 '
 
+CheckOptions:
+- key:             performance-for-range-copy.WarnOnAllAutoCopies
+  value:           true
+- key:             performance-unnecessary-value-param.AllowedTypes
+  value:           'exception_ptr$;'
+- key:             readability-implicit-bool-conversion.AllowPointerConditions
+  value:           true
+
 HeaderFilterRegex: 'pybind11/.*h'
+
+WarningsAsErrors: '*'

wrap/pybind11/.github/CODEOWNERS

@@ -0,0 +1,9 @@
+*.cmake @henryiii
+CMakeLists.txt @henryiii
+*.yml @henryiii
+*.yaml @henryiii
+/tools/ @henryiii
+/pybind11/ @henryiii
+noxfile.py @henryiii
+.clang-format @henryiii
+.clang-tidy @henryiii

wrap/pybind11/.github/CONTRIBUTING.md

@@ -53,6 +53,33 @@ derivative works thereof, in binary and source code form.
 
 ## Development of pybind11
 
+### Quick setup
+
+To setup a quick development environment, use [`nox`](https://nox.thea.codes).
+This will allow you to do some common tasks with minimal setup effort, but will
+take more time to run and be less flexible than a full development environment.
+If you use [`pipx run nox`](https://pipx.pypa.io), you don't even need to
+install `nox`. Examples:
+
+```bash
+# List all available sessions
+nox -l
+
+# Run linters
+nox -s lint
+
+# Run tests on Python 3.9
+nox -s tests-3.9
+
+# Build and preview docs
+nox -s docs -- serve
+
+# Build SDists and wheels
+nox -s build
+```
+
+### Full setup
+
 To setup an ideal development environment, run the following commands on a
 system with CMake 3.14+:
 
@@ -93,7 +120,7 @@ The valid options are:
 * `-DPYBIND11_NOPYTHON=ON`: Disable all Python searching (disables tests)
 * `-DBUILD_TESTING=ON`: Enable the tests
 * `-DDOWNLOAD_CATCH=ON`: Download catch to build the C++ tests
-* `-DOWNLOAD_EIGEN=ON`: Download Eigen for the NumPy tests
+* `-DDOWNLOAD_EIGEN=ON`: Download Eigen for the NumPy tests
 * `-DPYBIND11_INSTALL=ON/OFF`: Enable the install target (on by default for the
   master project)
 * `-DUSE_PYTHON_INSTALL_DIR=ON`: Try to install into the python dir
@@ -126,13 +153,26 @@ cmake --build build --target check
 `--target` can be spelled `-t` in CMake 3.15+. You can also run individual
 tests with these targets:
 
-* `pytest`: Python tests only
+* `pytest`: Python tests only, using the
+[pytest](https://docs.pytest.org/en/stable/) framework
 * `cpptest`: C++ tests only
 * `test_cmake_build`: Install / subdirectory tests
 
 If you want to build just a subset of tests, use
-`-DPYBIND11_TEST_OVERRIDE="test_callbacks.cpp;test_pickling.cpp"`. If this is
+`-DPYBIND11_TEST_OVERRIDE="test_callbacks;test_pickling"`. If this is
-empty, all tests will be built.
+empty, all tests will be built. Tests are specified without an extension if they need both a .py and
+.cpp file.
+
+You may also pass flags to the `pytest` target by editing `tests/pytest.ini` or
+by using the `PYTEST_ADDOPTS` environment variable
+(see [`pytest` docs](https://docs.pytest.org/en/2.7.3/customize.html#adding-default-options)). As an example:
+
+```bash
+env PYTEST_ADDOPTS="--capture=no --exitfirst" \
+    cmake --build build --target pytest
+# Or using abbreviated flags
+env PYTEST_ADDOPTS="-s -x" cmake --build build --target pytest
+```
 
 ### Formatting
 
@@ -164,16 +204,42 @@ name, pre-commit):
 pre-commit install
 ```
 
-### Clang-Tidy
+### Clang-Format
 
-To run Clang tidy, the following recipe should work. Files will be modified in
+As of v2.6.2, pybind11 ships with a [`clang-format`][clang-format]
-place, so you can use git to monitor the changes.
+configuration file at the top level of the repo (the filename is
+`.clang-format`). Currently, formatting is NOT applied automatically, but
+manually using `clang-format` for newly developed files is highly encouraged.
+To check if a file needs formatting:
 
 ```bash
-docker run --rm -v $PWD:/pybind11 -it silkeh/clang:10
+clang-format -style=file --dry-run some.cpp
-apt-get update && apt-get install python3-dev python3-pytest
+```
-cmake -S pybind11/ -B build -DCMAKE_CXX_CLANG_TIDY="$(which clang-tidy);-fix"
+
-cmake --build build
+The output will show things to be fixed, if any. To actually format the file:
+
+```bash
+clang-format -style=file -i some.cpp
+```
+
+Note that the `-style-file` option searches the parent directories for the
+`.clang-format` file, i.e. the commands above can be run in any subdirectory
+of the pybind11 repo.
+
+### Clang-Tidy
+
+[`clang-tidy`][clang-tidy] performs deeper static code analyses and is
+more complex to run, compared to `clang-format`, but support for `clang-tidy`
+is built into the pybind11 CMake configuration. To run `clang-tidy`, the
+following recipe should work. Run the `docker` command from the top-level
+directory inside your pybind11 git clone. Files will be modified in place,
+so you can use git to monitor the changes.
+
+```bash
+docker run --rm -v $PWD:/mounted_pybind11 -it silkeh/clang:12
+apt-get update && apt-get install -y python3-dev python3-pytest
+cmake -S /mounted_pybind11/ -B build -DCMAKE_CXX_CLANG_TIDY="$(which clang-tidy);-fix" -DDOWNLOAD_EIGEN=ON -DDOWNLOAD_CATCH=ON -DCMAKE_CXX_STANDARD=17
+cmake --build build -j 2 -- --keep-going
 ```
 
 ### Include what you use
@@ -186,7 +252,7 @@ cmake -S . -B build-iwyu -DCMAKE_CXX_INCLUDE_WHAT_YOU_USE=$(which include-what-y
 cmake --build build
 ```
 
-The report is sent to stderr; you can pip it into a file if you wish.
+The report is sent to stderr; you can pipe it into a file if you wish.
 
 ### Build recipes
 
@@ -313,6 +379,8 @@ if you really want to.
 
 
 [pre-commit]: https://pre-commit.com
+[clang-format]: https://clang.llvm.org/docs/ClangFormat.html
+[clang-tidy]: https://clang.llvm.org/extra/clang-tidy/
 [pybind11.readthedocs.org]: http://pybind11.readthedocs.org/en/latest
 [issue tracker]: https://github.com/pybind/pybind11/issues
 [gitter]: https://gitter.im/pybind/Lobby

wrap/pybind11/.github/ISSUE_TEMPLATE/bug-report.md

@@ -1,28 +0,0 @@
----
-name: Bug Report
-about: File an issue about a bug
-title: "[BUG] "
----
-
-
-Make sure you've completed the following steps before submitting your issue -- thank you!
-
-1. Make sure you've read the [documentation][]. Your issue may be addressed there.
-2. Search the [issue tracker][] to verify that this hasn't already been reported. +1 or comment there if it has.
-3. Consider asking first in the [Gitter chat room][].
-4. Include a self-contained and minimal piece of code that reproduces the problem. If that's not possible, try to make the description as clear as possible.
-    a. If possible, make a PR with a new, failing test to give us a starting point to work on!
-
-[documentation]: https://pybind11.readthedocs.io
-[issue tracker]: https://github.com/pybind/pybind11/issues
-[Gitter chat room]: https://gitter.im/pybind/Lobby
-
-*After reading, remove this checklist and the template text in parentheses below.*
-
-## Issue description
-
-(Provide a short description, state the expected behavior and what actually happens.)
-
-## Reproducible example code
-
-(The code should be minimal, have no external dependencies, isolate the function(s) that cause breakage. Submit matched and complete C++ and Python snippets that can be easily compiled and run to diagnose the issue.)

wrap/pybind11/.github/ISSUE_TEMPLATE/bug-report.yml

@@ -0,0 +1,45 @@
+name: Bug Report
+description: File an issue about a bug
+title: "[BUG]: "
+labels: [triage]
+body:
+  - type: markdown
+    attributes:
+      value: |
+        Maintainers will only make a best effort to triage PRs. Please do your best to make the issue as easy to act on as possible, and only open if clearly a problem with pybind11 (ask first if unsure).
+  - type: checkboxes
+    id: steps
+    attributes:
+      label: Required prerequisites
+      description: Make sure you've completed the following steps before submitting your issue -- thank you!
+      options:
+        - label: Make sure you've read the [documentation](https://pybind11.readthedocs.io). Your issue may be addressed there.
+          required: true
+        - label: Search the [issue tracker](https://github.com/pybind/pybind11/issues) and [Discussions](https:/pybind/pybind11/discussions) to verify that this hasn't already been reported. +1 or comment there if it has.
+          required: true
+        - label: Consider asking first in the [Gitter chat room](https://gitter.im/pybind/Lobby) or in a [Discussion](https:/pybind/pybind11/discussions/new).
+          required: false
+
+  - type: textarea
+    id: description
+    attributes:
+      label: Problem description
+      placeholder: >-
+        Provide a short description, state the expected behavior and what
+        actually happens. Include relevant information like what version of
+        pybind11 you are using, what system you are on, and any useful commands
+        / output.
+    validations:
+      required: true
+
+  - type: textarea
+    id: code
+    attributes:
+      label: Reproducible example code
+      placeholder: >-
+        The code should be minimal, have no external dependencies, isolate the
+        function(s) that cause breakage. Submit matched and complete C++ and
+        Python snippets that can be easily compiled and run to diagnose the
+        issue. If possible, make a PR with a new, failing test to give us a
+        starting point to work on!
+      render: text

wrap/pybind11/.github/ISSUE_TEMPLATE/config.yml

@@ -1,5 +1,8 @@
 blank_issues_enabled: false
 contact_links:
+  - name: Ask a question
+    url: https://github.com/pybind/pybind11/discussions/new
+    about: Please ask and answer questions here, or propose new ideas.
   - name: Gitter room
     url: https://gitter.im/pybind/Lobby
     about: A room for discussing pybind11 with an active community

wrap/pybind11/.github/ISSUE_TEMPLATE/feature-request.md

@@ -1,16 +0,0 @@
----
-name: Feature Request
-about: File an issue about adding a feature
-title: "[FEAT] "
----
-
-
-Make sure you've completed the following steps before submitting your issue -- thank you!
-
-1. Check if your feature has already been mentioned / rejected / planned in other issues.
-2. If those resources didn't help, consider asking in the [Gitter chat room][] to see if this is interesting / useful to a larger audience and possible to implement reasonably,
-4. If you have a useful feature that passes the previous items (or not suitable for chat), please fill in the details below.
-
-[Gitter chat room]: https://gitter.im/pybind/Lobby
-
-*After reading, remove this checklist.*

wrap/pybind11/.github/ISSUE_TEMPLATE/question.md

@@ -1,21 +0,0 @@
----
-name: Question
-about: File an issue about unexplained behavior
-title: "[QUESTION] "
----
-
-If you have a question, please check the following first:
-
-1. Check if your question has already been answered in the [FAQ][] section.
-2. Make sure you've read the [documentation][]. Your issue may be addressed there.
-3. If those resources didn't help and you only have a short question (not a bug report), consider asking in the [Gitter chat room][]
-4. Search the [issue tracker][], including the closed issues, to see if your question has already been asked/answered. +1 or comment if it has been asked but has no answer.
-5. If you have a more complex question which is not answered in the previous items (or not suitable for chat), please fill in the details below.
-6. Include a self-contained and minimal piece of code that illustrates your question. If that's not possible, try to make the description as clear as possible.
-
-[FAQ]: http://pybind11.readthedocs.io/en/latest/faq.html
-[documentation]: https://pybind11.readthedocs.io
-[issue tracker]: https://github.com/pybind/pybind11/issues
-[Gitter chat room]: https://gitter.im/pybind/Lobby
-
-*After reading, remove this checklist.*

wrap/pybind11/.github/dependabot.yml

@@ -0,0 +1,16 @@
+version: 2
+updates:
+  # Maintain dependencies for GitHub Actions
+  - package-ecosystem: "github-actions"
+    directory: "/"
+    schedule:
+      interval: "daily"
+    ignore:
+      # Official actions have moving tags like v1
+      # that are used, so they don't need updates here
+      - dependency-name: "actions/checkout"
+      - dependency-name: "actions/setup-python"
+      - dependency-name: "actions/cache"
+      - dependency-name: "actions/upload-artifact"
+      - dependency-name: "actions/download-artifact"
+      - dependency-name: "actions/labeler"

wrap/pybind11/.github/labeler.yml

@@ -0,0 +1,8 @@
+docs:
+- any:
+  - 'docs/**/*.rst'
+  - '!docs/changelog.rst'
+  - '!docs/upgrade.rst'
+
+ci:
+- '.github/workflows/*.yml'

wrap/pybind11/.github/labeler_merged.yml

@@ -0,0 +1,3 @@
+needs changelog:
+- all:
+  - '!docs/changelog.rst'

wrap/pybind11/.github/pull_request_template.md

@@ -0,0 +1,19 @@
+<!--
+Title (above): please place [branch_name] at the beginning if you are targeting a branch other than master. *Do not target stable*.
+It is recommended to use conventional commit format, see conventionalcommits.org, but not required.
+-->
+## Description
+
+<!-- Include relevant issues or PRs here, describe what changed and why -->
+
+
+## Suggested changelog entry:
+
+<!-- Fill in the below block with the expected RestructuredText entry. Delete if no entry needed;
+     but do not delete header or rst block if an entry is needed! Will be collected via a script. -->
+
+```rst
+
+```
+
+<!-- If the upgrade guide needs updating, note that here too -->

wrap/pybind11/.github/workflows/ci.yml

@@ -9,6 +9,13 @@ on:
       - stable
       - v*
 
+concurrency:
+  group: test-${{ github.ref }}
+  cancel-in-progress: true
+
+env:
+  PIP_ONLY_BINARY: numpy
+
 jobs:
   # This is the "main" test suite, which tests a large number of different
   # versions of default compilers and Python versions in GitHub Actions.
@@ -16,71 +23,42 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        runs-on: [ubuntu-latest, windows-latest, macos-latest]
+        runs-on: [ubuntu-latest, windows-2022, macos-latest]
-        arch: [x64]
         python:
-        - 2.7
+        - '2.7'
-        - 3.5
+        - '3.5'
-        - 3.8
+        - '3.6'
-        - pypy2
+        - '3.9'
-        - pypy3
+        - '3.10'
+        - 'pypy-3.7-v7.3.7'
+        - 'pypy-3.8-v7.3.7'
 
         # Items in here will either be added to the build matrix (if not
         # present), or add new keys to an existing matrix element if all the
         # existing keys match.
         #
-        # We support three optional keys: args (both build), args1 (first
+        # We support an optional key: args, for cmake args
-        # build), and args2 (second build).
         include:
+          # Just add a key
           - runs-on: ubuntu-latest
-            python: 3.6
+            python: '3.6'
-            arch: x64
             args: >
               -DPYBIND11_FINDPYTHON=ON
-          - runs-on: windows-2016
+              -DCMAKE_CXX_FLAGS="-D_=1"
-            python: 3.7
-            arch: x86
-            args2: >
-              -DCMAKE_CXX_FLAGS="/permissive- /EHsc /GR"
           - runs-on: windows-latest
-            python: 3.6
+            python: '3.6'
-            arch: x64
             args: >
               -DPYBIND11_FINDPYTHON=ON
-          - runs-on: windows-latest
-            python: 3.7
-            arch: x64
-
-          - runs-on: ubuntu-latest
-            python: 3.9-dev
-            arch: x64
           - runs-on: macos-latest
-            python: 3.9-dev
+            python: 'pypy-2.7'
-            arch: x64
+          # Inject a couple Windows 2019 runs
-            args: >
+          - runs-on: windows-2019
-              -DPYBIND11_FINDPYTHON=ON
+            python: '3.9'
+          - runs-on: windows-2019
+            python: '2.7'
 
-        # These items will be removed from the build matrix, keys must match.
+    name: "🐍 ${{ matrix.python }} • ${{ matrix.runs-on }} • x64 ${{ matrix.args }}"
-        exclude:
-            # Currently 32bit only, and we build 64bit
-          - runs-on: windows-latest
-            python: pypy2
-            arch: x64
-          - runs-on: windows-latest
-            python: pypy3
-            arch: x64
-
-            # Currently broken on embed_test
-          - runs-on: windows-latest
-            python: 3.8
-            arch: x64
-          - runs-on: windows-latest
-            python: 3.9-dev
-            arch: x64
-
-    name: "🐍 ${{ matrix.python }} • ${{ matrix.runs-on }} • ${{ matrix.arch }} ${{ matrix.args }}"
     runs-on: ${{ matrix.runs-on }}
-    continue-on-error: ${{ endsWith(matrix.python, 'dev') }}
 
     steps:
     - uses: actions/checkout@v2
@@ -89,13 +67,18 @@ jobs:
       uses: actions/setup-python@v2
       with:
         python-version: ${{ matrix.python }}
-        architecture: ${{ matrix.arch }}
 
-    - name: Setup Boost (Windows / Linux latest)
+    - name: Setup Boost (Linux)
-      run: echo "::set-env name=BOOST_ROOT::$BOOST_ROOT_1_72_0"
+      # Can't use boost + define _
+      if: runner.os == 'Linux' && matrix.python != '3.6'
+      run: sudo apt-get install libboost-dev
+
+    - name: Setup Boost (macOS)
+      if: runner.os == 'macOS'
+      run: brew install boost
 
     - name: Update CMake
-      uses: jwlawson/actions-setup-cmake@v1.3
+      uses: jwlawson/actions-setup-cmake@v1.12
 
     - name: Cache wheels
       if: runner.os == 'macOS'
@@ -106,10 +89,11 @@ jobs:
         # for ways to do this more generally
         path: ~/Library/Caches/pip
         # Look to see if there is a cache hit for the corresponding requirements file
-        key: ${{ runner.os }}-pip-${{ matrix.python }}-${{ matrix.arch }}-${{ hashFiles('tests/requirements.txt') }}
+        key: ${{ runner.os }}-pip-${{ matrix.python }}-x64-${{ hashFiles('tests/requirements.txt') }}
 
     - name: Prepare env
-      run: python -m pip install -r tests/requirements.txt --prefer-binary
+      run: |
+        python -m pip install -r tests/requirements.txt
 
     - name: Setup annotations on Linux
       if: runner.os == 'Linux'
@@ -132,6 +116,8 @@ jobs:
       run: cmake --build . --target pytest -j 2
 
     - name: C++11 tests
+      # TODO: Figure out how to load the DLL on Python 3.8+
+      if: "!(runner.os == 'Windows' && (matrix.python == 3.8 || matrix.python == 3.9 || matrix.python == '3.10' || matrix.python == '3.11-dev' || matrix.python == 'pypy-3.8'))"
       run: cmake --build .  --target cpptest -j 2
 
     - name: Interface test C++11
@@ -141,7 +127,7 @@ jobs:
       run: git clean -fdx
 
     # Second build - C++17 mode and in a build directory
-    - name: Configure ${{ matrix.args2 }}
+    - name: Configure C++17
       run: >
         cmake -S . -B build2
         -DPYBIND11_WERROR=ON
@@ -149,7 +135,6 @@ jobs:
         -DDOWNLOAD_EIGEN=ON
         -DCMAKE_CXX_STANDARD=17
         ${{ matrix.args }}
-        ${{ matrix.args2 }}
 
     - name: Build
       run: cmake --build build2 -j 2
@@ -158,8 +143,28 @@ jobs:
       run: cmake --build build2 --target pytest
 
     - name: C++ tests
+      # TODO: Figure out how to load the DLL on Python 3.8+
+      if: "!(runner.os == 'Windows' && (matrix.python == 3.8 || matrix.python == 3.9 || matrix.python == '3.10' || matrix.python == '3.11-dev' || matrix.python == 'pypy-3.8'))"
       run: cmake --build build2 --target cpptest
 
+    # Third build - C++17 mode with unstable ABI
+    - name: Configure (unstable ABI)
+      run: >
+        cmake -S . -B build3
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=17
+        -DPYBIND11_INTERNALS_VERSION=10000000
+        "-DPYBIND11_TEST_OVERRIDE=test_call_policies.cpp;test_gil_scoped.cpp;test_thread.cpp"
+        ${{ matrix.args }}
+
+    - name: Build (unstable ABI)
+      run: cmake --build build3 -j 2
+
+    - name: Python tests (unstable ABI)
+      run: cmake --build build3 --target pytest
+
     - name: Interface test
       run: cmake --build build2 --target test_cmake_build
 
@@ -167,21 +172,105 @@ jobs:
     # MSVC, but for now, this action works:
     - name: Prepare compiler environment for Windows 🐍 2.7
       if: matrix.python == 2.7 && runner.os == 'Windows'
-      uses: ilammy/msvc-dev-cmd@v1
+      uses: ilammy/msvc-dev-cmd@v1.10.0
       with:
         arch: x64
 
     # This makes two environment variables available in the following step(s)
     - name: Set Windows 🐍 2.7 environment variables
       if: matrix.python == 2.7 && runner.os == 'Windows'
+      shell: bash
       run: |
-        echo "::set-env name=DISTUTILS_USE_SDK::1"
+        echo "DISTUTILS_USE_SDK=1" >> $GITHUB_ENV
-        echo "::set-env name=MSSdk::1"
+        echo "MSSdk=1" >> $GITHUB_ENV
 
     # This makes sure the setup_helpers module can build packages using
     # setuptools
     - name: Setuptools helpers test
       run: pytest tests/extra_setuptools
+      if: "!(matrix.python == '3.5' && matrix.runs-on == 'windows-2022')"
+
+
+  deadsnakes:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+        # TODO: Fails on 3.10, investigate
+        - python-version: "3.9"
+          python-debug: true
+          valgrind: true
+      # - python-version: "3.11-dev"
+      #   python-debug: false
+
+    name: "🐍 ${{ matrix.python-version }}${{ matrix.python-debug && '-dbg' || '' }} (deadsnakes)${{ matrix.valgrind && ' • Valgrind' || '' }} • x64"
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup Python ${{ matrix.python-version }} (deadsnakes)
+      uses: deadsnakes/action@v2.1.1
+      with:
+        python-version: ${{ matrix.python-version }}
+        debug: ${{ matrix.python-debug }}
+
+    - name: Update CMake
+      uses: jwlawson/actions-setup-cmake@v1.12
+
+    - name: Valgrind cache
+      if: matrix.valgrind
+      uses: actions/cache@v2
+      id: cache-valgrind
+      with:
+        path: valgrind
+        key: 3.16.1 # Valgrind version
+
+    - name: Compile Valgrind
+      if: matrix.valgrind && steps.cache-valgrind.outputs.cache-hit != 'true'
+      run: |
+        VALGRIND_VERSION=3.16.1
+        curl https://sourceware.org/pub/valgrind/valgrind-$VALGRIND_VERSION.tar.bz2 -o - | tar xj
+        mv valgrind-$VALGRIND_VERSION valgrind
+        cd valgrind
+        ./configure
+        make -j 2 > /dev/null
+
+    - name: Install Valgrind
+      if: matrix.valgrind
+      working-directory: valgrind
+      run: |
+        sudo make install
+        sudo apt-get update
+        sudo apt-get install libc6-dbg  # Needed by Valgrind
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    - name: Configure
+      env:
+        SETUPTOOLS_USE_DISTUTILS: stdlib
+      run: >
+        cmake -S . -B build
+        -DCMAKE_BUILD_TYPE=Debug
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=17
+
+    - name: Build
+      run: cmake --build build -j 2
+
+    - name: Python tests
+      run: cmake --build build --target pytest
+
+    - name: C++ tests
+      run: cmake --build build --target cpptest
+
+    - name: Run Valgrind on Python tests
+      if: matrix.valgrind
+      run: cmake --build build --target memcheck
 
 
   # Testing on clang using the excellent silkeh clang docker images
@@ -194,12 +283,20 @@ jobs:
           - 3.6
           - 3.7
           - 3.9
-          - 5
           - 7
           - 9
           - dev
+        std:
+          - 11
+        include:
+          - clang: 5
+            std: 14
+          - clang: 10
+            std: 20
+          - clang: 10
+            std: 17
 
-    name: "🐍 3 • Clang ${{ matrix.clang }} • x64"
+    name: "🐍 3 • Clang ${{ matrix.clang }} • C++${{ matrix.std }} • x64"
     container: "silkeh/clang:${{ matrix.clang }}"
 
     steps:
@@ -214,6 +311,7 @@ jobs:
         cmake -S . -B build
         -DPYBIND11_WERROR=ON
         -DDOWNLOAD_CATCH=ON
+        -DCMAKE_CXX_STANDARD=${{ matrix.std }}
         -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
 
     - name: Build
@@ -252,50 +350,54 @@ jobs:
       run: cmake --build build --target pytest
 
 
-  # Testing CentOS 8 + PGI compilers
+# TODO: Internal compiler error - report to NVidia
-  centos-nvhpc8:
+#  # Testing CentOS 8 + PGI compilers
-    runs-on: ubuntu-latest
+#  centos-nvhpc8:
-    name: "🐍 3 • CentOS8 / PGI 20.7 • x64"
+#    runs-on: ubuntu-latest
-    container: centos:8
+#    name: "🐍 3 • CentOS8 / PGI 20.11 • x64"
-
+#    container: centos:8
-    steps:
+#
-    - uses: actions/checkout@v2
+#    steps:
-
+#    - uses: actions/checkout@v2
-    - name: Add Python 3 and a few requirements
+#
-      run: yum update -y && yum install -y git python3-devel python3-numpy python3-pytest make environment-modules
+#    - name: Add Python 3 and a few requirements
-
+#      run: yum update -y && yum install -y git python3-devel python3-numpy python3-pytest make environment-modules
-    - name: Install CMake with pip
+#
-      run: |
+#    - name: Install CMake with pip
-        python3 -m pip install --upgrade pip
+#      run: |
-        python3 -m pip install cmake --prefer-binary
+#        python3 -m pip install --upgrade pip
-
+#        python3 -m pip install cmake --prefer-binary
-    - name: Install NVidia HPC SDK
+#
-      run: yum -y install https://developer.download.nvidia.com/hpc-sdk/nvhpc-20-7-20.7-1.x86_64.rpm https://developer.download.nvidia.com/hpc-sdk/nvhpc-2020-20.7-1.x86_64.rpm
+#    - name: Install NVidia HPC SDK
-
+#      run: >
-    - name: Configure
+#        yum -y install
-      shell: bash
+#        https://developer.download.nvidia.com/hpc-sdk/20.11/nvhpc-20-11-20.11-1.x86_64.rpm
-      run: |
+#        https://developer.download.nvidia.com/hpc-sdk/20.11/nvhpc-2020-20.11-1.x86_64.rpm
-        source /etc/profile.d/modules.sh
+#
-        module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/20.7
+#    - name: Configure
-        cmake -S . -B build -DDOWNLOAD_CATCH=ON -DCMAKE_CXX_STANDARD=14 -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
+#      shell: bash
-
+#      run: |
-    - name: Build
+#        source /etc/profile.d/modules.sh
-      run: cmake --build build -j 2 --verbose
+#        module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/20.11
-
+#        cmake -S . -B build -DDOWNLOAD_CATCH=ON -DCMAKE_CXX_STANDARD=14 -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
-    - name: Python tests
+#
-      run: cmake --build build --target pytest
+#    - name: Build
-
+#      run: cmake --build build -j 2 --verbose
-    - name: C++ tests
+#
-      run: cmake --build build --target cpptest
+#    - name: Python tests
-
+#      run: cmake --build build --target pytest
-    - name: Interface test
+#
-      run: cmake --build build --target test_cmake_build
+#    - name: C++ tests
+#      run: cmake --build build --target cpptest
+#
+#    - name: Interface test
+#      run: cmake --build build --target test_cmake_build
 
 
   # Testing on CentOS 7 + PGI compilers, which seems to require more workarounds
   centos-nvhpc7:
     runs-on: ubuntu-latest
-    name: "🐍 3 • CentOS7 / PGI 20.7 • x64"
+    name: "🐍 3 • CentOS7 / PGI 20.9 • x64"
     container: centos:7
 
     steps:
@@ -305,17 +407,17 @@ jobs:
       run: yum update -y && yum install -y epel-release && yum install -y git python3-devel make environment-modules cmake3
 
     - name: Install NVidia HPC SDK
-      run:  yum -y install https://developer.download.nvidia.com/hpc-sdk/nvhpc-20-7-20.7-1.x86_64.rpm https://developer.download.nvidia.com/hpc-sdk/nvhpc-2020-20.7-1.x86_64.rpm
+      run:  yum -y install https://developer.download.nvidia.com/hpc-sdk/20.9/nvhpc-20-9-20.9-1.x86_64.rpm https://developer.download.nvidia.com/hpc-sdk/20.9/nvhpc-2020-20.9-1.x86_64.rpm
 
     # On CentOS 7, we have to filter a few tests (compiler internal error)
-    # and allow deeper templete recursion (not needed on CentOS 8 with a newer
+    # and allow deeper template recursion (not needed on CentOS 8 with a newer
     # standard library). On some systems, you many need further workarounds:
     # https://github.com/pybind/pybind11/pull/2475
     - name: Configure
       shell: bash
       run: |
         source /etc/profile.d/modules.sh
-        module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/20.7
+        module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/20.9
         cmake3 -S . -B build -DDOWNLOAD_CATCH=ON \
                             -DCMAKE_CXX_STANDARD=11 \
                             -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") \
@@ -340,6 +442,7 @@ jobs:
     - name: Interface test
       run: cmake3 --build build --target test_cmake_build
 
+
   # Testing on GCC using the GCC docker images (only recent images supported)
   gcc:
     runs-on: ubuntu-latest
@@ -349,8 +452,13 @@ jobs:
         gcc:
           - 7
           - latest
+        std:
+          - 11
+        include:
+          - gcc: 10
+            std: 20
 
-    name: "🐍 3 • GCC ${{ matrix.gcc }} • x64"
+    name: "🐍 3 • GCC ${{ matrix.gcc }} • C++${{ matrix.std }}• x64"
     container: "gcc:${{ matrix.gcc }}"
 
     steps:
@@ -362,10 +470,8 @@ jobs:
     - name: Update pip
       run: python3 -m pip install --upgrade pip
 
-    - name: Setup CMake 3.18
+    - name: Update CMake
-      uses: jwlawson/actions-setup-cmake@v1.3
+      uses: jwlawson/actions-setup-cmake@v1.12
-      with:
-        cmake-version: 3.18
 
     - name: Configure
       shell: bash
@@ -373,7 +479,7 @@ jobs:
         cmake -S . -B build
         -DPYBIND11_WERROR=ON
         -DDOWNLOAD_CATCH=ON
-        -DCMAKE_CXX_STANDARD=11
+        -DCMAKE_CXX_STANDARD=${{ matrix.std }}
         -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
 
     - name: Build
@@ -389,6 +495,103 @@ jobs:
       run: cmake --build build --target test_cmake_build
 
 
+  # Testing on ICC using the oneAPI apt repo
+  icc:
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+
+    name: "🐍 3 • ICC latest • x64"
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Add apt repo
+      run: |
+        sudo apt-get update
+        sudo apt-get install -y wget build-essential pkg-config cmake ca-certificates gnupg
+        wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS-2023.PUB
+        sudo apt-key add GPG-PUB-KEY-INTEL-SW-PRODUCTS-2023.PUB
+        echo "deb https://apt.repos.intel.com/oneapi all main" | sudo tee /etc/apt/sources.list.d/oneAPI.list
+
+    - name: Add ICC & Python 3
+      run: sudo apt-get update; sudo apt-get install -y intel-oneapi-compiler-dpcpp-cpp-and-cpp-classic cmake python3-dev python3-numpy python3-pytest python3-pip
+
+    - name: Update pip
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        python3 -m pip install --upgrade pip
+
+    - name: Install dependencies
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        python3 -m pip install -r tests/requirements.txt
+
+    - name: Configure C++11
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake -S . -B build-11     \
+        -DPYBIND11_WERROR=ON    \
+        -DDOWNLOAD_CATCH=ON     \
+        -DDOWNLOAD_EIGEN=OFF    \
+        -DCMAKE_CXX_STANDARD=11             \
+        -DCMAKE_CXX_COMPILER=$(which icpc)  \
+        -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
+
+    - name: Build C++11
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-11 -j 2 -v
+
+    - name: Python tests C++11
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        sudo service apport stop
+        cmake --build build-11 --target check
+
+    - name: C++ tests C++11
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-11 --target cpptest
+
+    - name: Interface test C++11
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-11 --target test_cmake_build
+
+    - name: Configure C++17
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake -S . -B build-17     \
+        -DPYBIND11_WERROR=ON    \
+        -DDOWNLOAD_CATCH=ON     \
+        -DDOWNLOAD_EIGEN=OFF    \
+        -DCMAKE_CXX_STANDARD=17             \
+        -DCMAKE_CXX_COMPILER=$(which icpc)  \
+        -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
+
+    - name: Build C++17
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-17 -j 2 -v
+
+    - name: Python tests C++17
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        sudo service apport stop
+        cmake --build build-17 --target check
+
+    - name: C++ tests C++17
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-17 --target cpptest
+
+    - name: Interface test C++17
+      run: |
+        set +e; source /opt/intel/oneapi/setvars.sh; set -e
+        cmake --build build-17 --target test_cmake_build
+
+
   # Testing on CentOS (manylinux uses a centos base, and this is an easy way
   # to get GCC 4.8, which is the manylinux1 compiler).
   centos:
@@ -397,11 +600,11 @@ jobs:
       fail-fast: false
       matrix:
         centos:
-          - 7  # GCC 4.8
+          - centos7  # GCC 4.8
-          - 8
+          - stream8
 
     name: "🐍 3 • CentOS ${{ matrix.centos }} • x64"
-    container: "centos:${{ matrix.centos }}"
+    container: "quay.io/centos/centos:${{ matrix.centos }}"
 
     steps:
     - uses: actions/checkout@v2
@@ -413,12 +616,14 @@ jobs:
       run: python3 -m pip install --upgrade pip
 
     - name: Install dependencies
-      run: python3 -m pip install cmake -r tests/requirements.txt --prefer-binary
+      run: |
+        python3 -m pip install cmake -r tests/requirements.txt
 
     - name: Configure
       shell: bash
       run: >
         cmake -S . -B build
+        -DCMAKE_BUILD_TYPE=MinSizeRel
         -DPYBIND11_WERROR=ON
         -DDOWNLOAD_CATCH=ON
         -DDOWNLOAD_EIGEN=ON
@@ -476,7 +681,7 @@ jobs:
         -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
       working-directory: /build-tests
 
-    - name: Run tests
+    - name: Python tests
       run: make pytest -j 2
       working-directory: /build-tests
 
@@ -493,16 +698,13 @@ jobs:
     - uses: actions/setup-python@v2
 
     - name: Install Doxygen
-      run: sudo apt install -y doxygen
+      run: sudo apt-get install -y doxygen librsvg2-bin # Changed to rsvg-convert in 20.04
-
-    - name: Install docs & setup requirements
-      run: python3 -m pip install -r docs/requirements.txt
 
     - name: Build docs
-      run: python3 -m sphinx -W -b html docs docs/.build
+      run: pipx run nox -s docs
 
     - name: Make SDist
-      run: python3 setup.py sdist
+      run: pipx run nox -s build -- --sdist
 
     - run: git status --ignored
 
@@ -514,6 +716,250 @@ jobs:
     - name: Compare Dists (headers only)
       working-directory: include
       run: |
-        python3 -m pip install --user -U ../dist/*
+        python3 -m pip install --user -U ../dist/*.tar.gz
         installed=$(python3 -c "import pybind11; print(pybind11.get_include() + '/pybind11')")
         diff -rq $installed ./pybind11
+
+  win32:
+    strategy:
+      fail-fast: false
+      matrix:
+        python:
+        - 3.5
+        - 3.6
+        - 3.7
+        - 3.8
+        - 3.9
+        - pypy-3.6
+
+        include:
+          - python: 3.9
+            args: -DCMAKE_CXX_STANDARD=20 -DDOWNLOAD_EIGEN=OFF
+          - python: 3.8
+            args: -DCMAKE_CXX_STANDARD=17
+
+    name: "🐍 ${{ matrix.python }} • MSVC 2019 • x86 ${{ matrix.args }}"
+    runs-on: windows-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup Python ${{ matrix.python }}
+      uses: actions/setup-python@v2
+      with:
+        python-version: ${{ matrix.python }}
+        architecture: x86
+
+    - name: Update CMake
+      uses: jwlawson/actions-setup-cmake@v1.12
+
+    - name: Prepare MSVC
+      uses: ilammy/msvc-dev-cmd@v1.10.0
+      with:
+        arch: x86
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    # First build - C++11 mode and inplace
+    - name: Configure ${{ matrix.args }}
+      run: >
+        cmake -S . -B build
+        -G "Visual Studio 16 2019" -A Win32
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        ${{ matrix.args }}
+    - name: Build C++11
+      run: cmake --build build -j 2
+
+    - name: Python tests
+      run: cmake --build build -t pytest
+
+  win32-msvc2015:
+    name: "🐍 ${{ matrix.python }} • MSVC 2015 • x64"
+    runs-on: windows-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        python:
+          - 2.7
+          - 3.6
+          - 3.7
+          # todo: check/cpptest does not support 3.8+ yet
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup 🐍 ${{ matrix.python }}
+      uses: actions/setup-python@v2
+      with:
+        python-version: ${{ matrix.python }}
+
+    - name: Update CMake
+      uses: jwlawson/actions-setup-cmake@v1.12
+
+    - name: Prepare MSVC
+      uses: ilammy/msvc-dev-cmd@v1.10.0
+      with:
+        toolset: 14.0
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    # First build - C++11 mode and inplace
+    - name: Configure
+      run: >
+        cmake -S . -B build
+        -G "Visual Studio 14 2015" -A x64
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+
+    - name: Build C++14
+      run: cmake --build build -j 2
+
+    - name: Run all checks
+      run: cmake --build build -t check
+
+
+  win32-msvc2017:
+    name: "🐍 ${{ matrix.python }} • MSVC 2017 • x64"
+    runs-on: windows-2016
+    strategy:
+      fail-fast: false
+      matrix:
+        python:
+          - 2.7
+          - 3.5
+          - 3.7
+        std:
+          - 14
+
+        include:
+          - python: 2.7
+            std: 17
+            args: >
+              -DCMAKE_CXX_FLAGS="/permissive- /EHsc /GR"
+          - python: 3.7
+            std: 17
+            args: >
+              -DCMAKE_CXX_FLAGS="/permissive- /EHsc /GR"
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup 🐍 ${{ matrix.python }}
+      uses: actions/setup-python@v2
+      with:
+        python-version: ${{ matrix.python }}
+
+    - name: Update CMake
+      uses: jwlawson/actions-setup-cmake@v1.12
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    # First build - C++11 mode and inplace
+    - name: Configure
+      run: >
+        cmake -S . -B build
+        -G "Visual Studio 15 2017" -A x64
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=${{ matrix.std }}
+        ${{ matrix.args }}
+
+    - name: Build ${{ matrix.std }}
+      run: cmake --build build -j 2
+
+    - name: Run all checks
+      run: cmake --build build -t check
+
+  mingw:
+    name: "🐍 3 • windows-latest • ${{ matrix.sys }}"
+    runs-on: windows-latest
+    defaults:
+      run:
+        shell: msys2 {0}
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - { sys: mingw64, env: x86_64 }
+          - { sys: mingw32, env: i686 }
+    steps:
+    - uses: msys2/setup-msys2@v2
+      with:
+        msystem: ${{matrix.sys}}
+        install: >-
+          git
+          mingw-w64-${{matrix.env}}-gcc
+          mingw-w64-${{matrix.env}}-python-pip
+          mingw-w64-${{matrix.env}}-python-numpy
+          mingw-w64-${{matrix.env}}-python-scipy
+          mingw-w64-${{matrix.env}}-cmake
+          mingw-w64-${{matrix.env}}-make
+          mingw-w64-${{matrix.env}}-python-pytest
+          mingw-w64-${{matrix.env}}-eigen3
+          mingw-w64-${{matrix.env}}-boost
+          mingw-w64-${{matrix.env}}-catch
+
+    - uses: actions/checkout@v2
+
+    - name: Configure C++11
+      # LTO leads to many undefined reference like
+      # `pybind11::detail::function_call::function_call(pybind11::detail::function_call&&)
+      run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=11 -S . -B build
+
+    - name: Build C++11
+      run: cmake --build build -j 2
+
+    - name: Python tests C++11
+      run: cmake --build build --target pytest -j 2
+
+    - name: C++11 tests
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build --target cpptest -j 2
+
+    - name: Interface test C++11
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build --target test_cmake_build
+
+    - name: Clean directory
+      run: git clean -fdx
+
+    - name: Configure C++14
+      run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=14 -S . -B build2
+
+    - name: Build C++14
+      run: cmake --build build2 -j 2
+
+    - name: Python tests C++14
+      run: cmake --build build2 --target pytest -j 2
+
+    - name: C++14 tests
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build2 --target cpptest -j 2
+
+    - name: Interface test C++14
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build2 --target test_cmake_build
+
+    - name: Clean directory
+      run: git clean -fdx
+
+    - name: Configure C++17
+      run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=17 -S . -B build3
+
+    - name: Build C++17
+      run: cmake --build build3 -j 2
+
+    - name: Python tests C++17
+      run: cmake --build build3 --target pytest -j 2
+
+    - name: C++17 tests
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build3 --target cpptest -j 2
+
+    - name: Interface test C++17
+      run: PYTHONHOME=/${{matrix.sys}} PYTHONPATH=/${{matrix.sys}} cmake --build build3 --target test_cmake_build

wrap/pybind11/.github/workflows/configure.yml

@@ -18,7 +18,7 @@ jobs:
       matrix:
         runs-on: [ubuntu-latest, macos-latest, windows-latest]
         arch: [x64]
-        cmake: [3.18]
+        cmake: ["3.21"]
 
         include:
         - runs-on: ubuntu-latest
@@ -55,7 +55,7 @@ jobs:
     # An action for adding a specific version of CMake:
     #   https://github.com/jwlawson/actions-setup-cmake
     - name: Setup CMake ${{ matrix.cmake }}
-      uses: jwlawson/actions-setup-cmake@v1.3
+      uses: jwlawson/actions-setup-cmake@v1.12
       with:
         cmake-version: ${{ matrix.cmake }}
 
@@ -82,57 +82,3 @@ jobs:
       working-directory: build dir
       if: github.event_name == 'workflow_dispatch'
       run: cmake --build . --config Release --target check
-
-  # This builds the sdists and wheels and makes sure the files are exactly as
-  # expected. Using Windows and Python 2.7, since that is often the most
-  # challenging matrix element.
-  test-packaging:
-    name: 🐍 2.7 • 📦 tests • windows-latest
-    runs-on: windows-latest
-
-    steps:
-    - uses: actions/checkout@v2
-
-    - name: Setup 🐍 2.7
-      uses: actions/setup-python@v2
-      with:
-        python-version: 2.7
-
-    - name: Prepare env
-      run: python -m pip install -r tests/requirements.txt --prefer-binary
-
-    - name: Python Packaging tests
-      run: pytest tests/extra_python_package/
-
-
-  # This runs the packaging tests and also builds and saves the packages as
-  # artifacts.
-  packaging:
-    name: 🐍 3.8 • 📦 & 📦 tests • ubuntu-latest
-    runs-on: ubuntu-latest
-
-    steps:
-    - uses: actions/checkout@v2
-
-    - name: Setup 🐍 3.8
-      uses: actions/setup-python@v2
-      with:
-        python-version: 3.8
-
-    - name: Prepare env
-      run: python -m pip install -r tests/requirements.txt build twine --prefer-binary
-
-    - name: Python Packaging tests
-      run: pytest tests/extra_python_package/
-
-    - name: Build SDist and wheels
-      run: |
-        python -m build -s -w .
-        PYBIND11_GLOBAL_SDIST=1 python -m build -s -w .
-
-    - name: Check metadata
-      run: twine check dist/*
-
-    - uses: actions/upload-artifact@v2
-      with:
-        path: dist/*

wrap/pybind11/.github/workflows/format.yml

@@ -19,15 +19,17 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - uses: actions/setup-python@v2
-    - uses: pre-commit/action@v2.0.0
+    - uses: pre-commit/action@v2.0.3
       with:
         # Slow hooks are marked with manual - slow is okay here, run them too
-        extra_args: --hook-stage manual
+        extra_args: --hook-stage manual --all-files
 
   clang-tidy:
+    # When making changes here, please also review the "Clang-Tidy" section
+    # in .github/CONTRIBUTING.md and update as needed.
     name: Clang-Tidy
     runs-on: ubuntu-latest
-    container: silkeh/clang:10
+    container: silkeh/clang:12
     steps:
     - uses: actions/checkout@v2
 
@@ -35,7 +37,12 @@ jobs:
       run: apt-get update && apt-get install -y python3-dev python3-pytest
 
     - name: Configure
-      run: cmake -S . -B build -DCMAKE_CXX_CLANG_TIDY="$(which clang-tidy);--warnings-as-errors=*"
+      run: >
+        cmake -S . -B build
+        -DCMAKE_CXX_CLANG_TIDY="$(which clang-tidy)"
+        -DDOWNLOAD_EIGEN=ON
+        -DDOWNLOAD_CATCH=ON
+        -DCMAKE_CXX_STANDARD=17
 
     - name: Build
-      run: cmake --build build -j 2
+      run: cmake --build build -j 2 -- --keep-going

wrap/pybind11/.github/workflows/labeler.yml

@@ -0,0 +1,16 @@
+name: Labeler
+on:
+  pull_request_target:
+    types: [closed]
+
+jobs:
+  label:
+    name: Labeler
+    runs-on: ubuntu-latest
+    steps:
+
+    - uses: actions/labeler@main
+      if: github.event.pull_request.merged == true
+      with:
+        repo-token: ${{ secrets.GITHUB_TOKEN }}
+        configuration-path: .github/labeler_merged.yml

wrap/pybind11/.github/workflows/pip.yml

@@ -0,0 +1,108 @@
+name: Pip
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+    - master
+    - stable
+    - v*
+  release:
+    types:
+    - published
+
+env:
+  PIP_ONLY_BINARY: numpy
+
+jobs:
+  # This builds the sdists and wheels and makes sure the files are exactly as
+  # expected. Using Windows and Python 2.7, since that is often the most
+  # challenging matrix element.
+  test-packaging:
+    name: 🐍 2.7 • 📦 tests • windows-latest
+    runs-on: windows-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup 🐍 2.7
+      uses: actions/setup-python@v2
+      with:
+        python-version: 2.7
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    - name: Python Packaging tests
+      run: pytest tests/extra_python_package/
+
+
+  # This runs the packaging tests and also builds and saves the packages as
+  # artifacts.
+  packaging:
+    name: 🐍 3.8 • 📦 & 📦 tests • ubuntu-latest
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup 🐍 3.8
+      uses: actions/setup-python@v2
+      with:
+        python-version: 3.8
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt build twine
+
+    - name: Python Packaging tests
+      run: pytest tests/extra_python_package/
+
+    - name: Build SDist and wheels
+      run: |
+        python -m build
+        PYBIND11_GLOBAL_SDIST=1 python -m build
+
+    - name: Check metadata
+      run: twine check dist/*
+
+    - name: Save standard package
+      uses: actions/upload-artifact@v2
+      with:
+        name: standard
+        path: dist/pybind11-*
+
+    - name: Save global package
+      uses: actions/upload-artifact@v2
+      with:
+        name: global
+        path: dist/pybind11_global-*
+
+
+
+  # When a GitHub release is made, upload the artifacts to PyPI
+  upload:
+    name: Upload to PyPI
+    runs-on: ubuntu-latest
+    if: github.event_name == 'release' && github.event.action == 'published'
+    needs: [packaging]
+
+    steps:
+    - uses: actions/setup-python@v2
+
+    # Downloads all to directories matching the artifact names
+    - uses: actions/download-artifact@v2
+
+    - name: Publish standard package
+      uses: pypa/gh-action-pypi-publish@v1.5.0
+      with:
+        password: ${{ secrets.pypi_password }}
+        packages_dir: standard/
+
+    - name: Publish global package
+      uses: pypa/gh-action-pypi-publish@v1.5.0
+      with:
+        password: ${{ secrets.pypi_password_global }}
+        packages_dir: global/

wrap/pybind11/.github/workflows/upstream.yml

@@ -0,0 +1,112 @@
+
+name: Upstream
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+concurrency:
+  group: upstream-${{ github.ref }}
+  cancel-in-progress: true
+
+env:
+  PIP_ONLY_BINARY: numpy
+
+jobs:
+  standard:
+    name: "🐍 3.11 dev • ubuntu-latest • x64"
+    runs-on: ubuntu-latest
+    if: "contains(github.event.pull_request.labels.*.name, 'python dev')"
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup Python 3.11
+      uses: actions/setup-python@v2
+      with:
+        python-version: "3.11-dev"
+
+    - name: Setup Boost (Linux)
+      if: runner.os == 'Linux'
+      run: sudo apt-get install libboost-dev
+
+    - name: Update CMake
+      uses: jwlawson/actions-setup-cmake@v1.12
+
+    - name: Prepare env
+      run: |
+        python -m pip install -r tests/requirements.txt
+
+    - name: Setup annotations on Linux
+      if: runner.os == 'Linux'
+      run: python -m pip install pytest-github-actions-annotate-failures
+
+    # First build - C++11 mode and inplace
+    - name: Configure C++11
+      run: >
+        cmake -S . -B .
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=11
+
+    - name: Build C++11
+      run: cmake --build . -j 2
+
+    - name: Python tests C++11
+      run: cmake --build . --target pytest -j 2
+
+    - name: C++11 tests
+      run: cmake --build .  --target cpptest -j 2
+
+    - name: Interface test C++11
+      run: cmake --build . --target test_cmake_build
+
+    - name: Clean directory
+      run: git clean -fdx
+
+    # Second build - C++17 mode and in a build directory
+    - name: Configure C++17
+      run: >
+        cmake -S . -B build2
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=17
+        ${{ matrix.args }}
+        ${{ matrix.args2 }}
+
+    - name: Build
+      run: cmake --build build2 -j 2
+
+    - name: Python tests
+      run: cmake --build build2 --target pytest
+
+    - name: C++ tests
+      run: cmake --build build2 --target cpptest
+
+    # Third build - C++17 mode with unstable ABI
+    - name: Configure (unstable ABI)
+      run: >
+        cmake -S . -B build3
+        -DPYBIND11_WERROR=ON
+        -DDOWNLOAD_CATCH=ON
+        -DDOWNLOAD_EIGEN=ON
+        -DCMAKE_CXX_STANDARD=17
+        -DPYBIND11_INTERNALS_VERSION=10000000
+        "-DPYBIND11_TEST_OVERRIDE=test_call_policies.cpp;test_gil_scoped.cpp;test_thread.cpp"
+        ${{ matrix.args }}
+
+    - name: Build (unstable ABI)
+      run: cmake --build build3 -j 2
+
+    - name: Python tests (unstable ABI)
+      run: cmake --build build3 --target pytest
+
+    - name: Interface test
+      run: cmake --build build2 --target test_cmake_build
+
+    # This makes sure the setup_helpers module can build packages using
+    # setuptools
+    - name: Setuptools helpers test
+      run: pytest tests/extra_setuptools

wrap/pybind11/.gitignore

@@ -41,3 +41,5 @@ pybind11Targets.cmake
 /.vscode
 /pybind11/include/*
 /pybind11/share/*
+/docs/_build/*
+.ipynb_checkpoints/

wrap/pybind11/.pre-commit-config.yaml

@@ -15,12 +15,14 @@
 repos:
 # Standard hooks
 - repo: https://github.com/pre-commit/pre-commit-hooks
-  rev: v3.2.0
+  rev: v4.1.0
   hooks:
   - id: check-added-large-files
   - id: check-case-conflict
+  - id: check-docstring-first
   - id: check-merge-conflict
   - id: check-symlinks
+  - id: check-toml
   - id: check-yaml
   - id: debug-statements
   - id: end-of-file-fixer
@@ -28,54 +30,115 @@ repos:
   - id: requirements-txt-fixer
   - id: trailing-whitespace
   - id: fix-encoding-pragma
+    exclude: ^noxfile.py$
+
+- repo: https://github.com/asottile/pyupgrade
+  rev: v2.31.0
+  hooks:
+  - id: pyupgrade
+
+- repo: https://github.com/PyCQA/isort
+  rev: 5.10.1
+  hooks:
+  - id: isort
 
 # Black, the code formatter, natively supports pre-commit
 - repo: https://github.com/psf/black
-  rev: 20.8b1
+  rev: 21.12b0 # Keep in sync with blacken-docs
   hooks:
   - id: black
-    # Not all Python files are Blacked, yet
+
-    files: ^(setup.py|pybind11|tests/extra)
+- repo: https://github.com/asottile/blacken-docs
+  rev: v1.12.0
+  hooks:
+  - id: blacken-docs
+    additional_dependencies:
+    - black==21.12b0 # keep in sync with black hook
 
 # Changes tabs to spaces
 - repo: https://github.com/Lucas-C/pre-commit-hooks
-  rev: v1.1.9
+  rev: v1.1.10
   hooks:
   - id: remove-tabs
 
+# Autoremoves unused imports
+- repo: https://github.com/hadialqattan/pycln
+  rev: v1.1.0
+  hooks:
+  - id: pycln
+
+- repo: https://github.com/pre-commit/pygrep-hooks
+  rev: v1.9.0
+  hooks:
+  - id: python-check-blanket-noqa
+  - id: python-check-blanket-type-ignore
+  - id: python-no-log-warn
+  - id: rst-backticks
+  - id: rst-directive-colons
+  - id: rst-inline-touching-normal
+
 # Flake8 also supports pre-commit natively (same author)
-- repo: https://gitlab.com/pycqa/flake8
+- repo: https://github.com/PyCQA/flake8
-  rev: 3.8.3
+  rev: 4.0.1
   hooks:
   - id: flake8
-    additional_dependencies: [flake8-bugbear, pep8-naming]
+    additional_dependencies: &flake8_dependencies
+      - flake8-bugbear
+      - pep8-naming
     exclude: ^(docs/.*|tools/.*)$
 
+- repo: https://github.com/asottile/yesqa
+  rev: v1.3.0
+  hooks:
+  - id: yesqa
+    additional_dependencies: *flake8_dependencies
+
 # CMake formatting
 - repo: https://github.com/cheshirekow/cmake-format-precommit
-  rev: v0.6.11
+  rev: v0.6.13
   hooks:
   - id: cmake-format
     additional_dependencies: [pyyaml]
     types: [file]
     files: (\.cmake|CMakeLists.txt)(.in)?$
 
+# Check static types with mypy
+- repo: https://github.com/pre-commit/mirrors-mypy
+  rev: v0.931
+  hooks:
+  - id: mypy
+    # Running per-file misbehaves a bit, so just run on all files, it's fast
+    pass_filenames: false
+    additional_dependencies: [typed_ast]
+
 # Checks the manifest for missing files (native support)
 - repo: https://github.com/mgedmin/check-manifest
-  rev: "0.42"
+  rev: "0.47"
   hooks:
   - id: check-manifest
     # This is a slow hook, so only run this if --hook-stage manual is passed
     stages: [manual]
     additional_dependencies: [cmake, ninja]
 
+- repo: https://github.com/codespell-project/codespell
+  rev: v2.1.0
+  hooks:
+  - id: codespell
+    exclude: ".supp$"
+    args: ["-L", "nd,ot,thist"]
+
+- repo: https://github.com/shellcheck-py/shellcheck-py
+  rev: v0.8.0.3
+  hooks:
+  - id: shellcheck
+
 # The original pybind11 checks for a few C++ style items
 - repo: local
   hooks:
   - id: disallow-caps
     name: Disallow improper capitalization
     language: pygrep
-    entry: PyBind|Numpy|Cmake
+    entry: PyBind|Numpy|Cmake|CCache|PyTest
     exclude: .pre-commit-config.yaml
 
 - repo: local

wrap/pybind11/CMakeLists.txt

@@ -7,13 +7,18 @@
 
 cmake_minimum_required(VERSION 3.4)
 
-# The `cmake_minimum_required(VERSION 3.4...3.18)` syntax does not work with
+# The `cmake_minimum_required(VERSION 3.4...3.22)` syntax does not work with
 # some versions of VS that have a patched CMake 3.11. This forces us to emulate
 # the behavior using the following workaround:
-if(${CMAKE_VERSION} VERSION_LESS 3.18)
+if(${CMAKE_VERSION} VERSION_LESS 3.22)
   cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION})
 else()
-  cmake_policy(VERSION 3.18)
+  cmake_policy(VERSION 3.22)
+endif()
+
+# Avoid infinite recursion if tests include this as a subdirectory
+if(DEFINED PYBIND11_MASTER_PROJECT)
+  return()
 endif()
 
 # Extract project version from source
@@ -73,6 +78,10 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
     set(CMAKE_CXX_EXTENSIONS OFF)
     set(CMAKE_CXX_STANDARD_REQUIRED ON)
   endif()
+
+  set(pybind11_system "")
+
+  set_property(GLOBAL PROPERTY USE_FOLDERS ON)
 else()
   set(PYBIND11_MASTER_PROJECT OFF)
   set(pybind11_system SYSTEM)
@@ -82,6 +91,9 @@ endif()
 option(PYBIND11_INSTALL "Install pybind11 header files?" ${PYBIND11_MASTER_PROJECT})
 option(PYBIND11_TEST "Build pybind11 test suite?" ${PYBIND11_MASTER_PROJECT})
 option(PYBIND11_NOPYTHON "Disable search for Python" OFF)
+set(PYBIND11_INTERNALS_VERSION
+    ""
+    CACHE STRING "Override the ABI version, may be used to enable the unstable ABI.")
 
 cmake_dependent_option(
   USE_PYTHON_INCLUDE_DIR
@@ -98,6 +110,7 @@ set(PYBIND11_HEADERS
     include/pybind11/detail/descr.h
     include/pybind11/detail/init.h
     include/pybind11/detail/internals.h
+    include/pybind11/detail/type_caster_base.h
     include/pybind11/detail/typeid.h
     include/pybind11/attr.h
     include/pybind11/buffer_info.h
@@ -109,6 +122,7 @@ set(PYBIND11_HEADERS
     include/pybind11/eigen.h
     include/pybind11/embed.h
     include/pybind11/eval.h
+    include/pybind11/gil.h
     include/pybind11/iostream.h
     include/pybind11/functional.h
     include/pybind11/numpy.h
@@ -116,7 +130,8 @@ set(PYBIND11_HEADERS
     include/pybind11/pybind11.h
     include/pybind11/pytypes.h
     include/pybind11/stl.h
-    include/pybind11/stl_bind.h)
+    include/pybind11/stl_bind.h
+    include/pybind11/stl/filesystem.h)
 
 # Compare with grep and warn if mismatched
 if(PYBIND11_MASTER_PROJECT AND NOT CMAKE_VERSION VERSION_LESS 3.12)
@@ -142,22 +157,45 @@ endif()
 string(REPLACE "include/" "${CMAKE_CURRENT_SOURCE_DIR}/include/" PYBIND11_HEADERS
                "${PYBIND11_HEADERS}")
 
-# Cache variables so pybind11_add_module can be used in parent projects
+# Cache variable so this can be used in parent projects
-set(PYBIND11_INCLUDE_DIR
+set(pybind11_INCLUDE_DIR
     "${CMAKE_CURRENT_LIST_DIR}/include"
-    CACHE INTERNAL "")
+    CACHE INTERNAL "Directory where pybind11 headers are located")
+
+# Backward compatible variable for add_subdirectory mode
+if(NOT PYBIND11_MASTER_PROJECT)
+  set(PYBIND11_INCLUDE_DIR
+      "${pybind11_INCLUDE_DIR}"
+      CACHE INTERNAL "")
+endif()
 
 # Note: when creating targets, you cannot use if statements at configure time -
 # you need generator expressions, because those will be placed in the target file.
 # You can also place ifs *in* the Config.in, but not here.
 
 # This section builds targets, but does *not* touch Python
+# Non-IMPORT targets cannot be defined twice
+if(NOT TARGET pybind11_headers)
+  # Build the headers-only target (no Python included):
+  # (long name used here to keep this from clashing in subdirectory mode)
+  add_library(pybind11_headers INTERFACE)
+  add_library(pybind11::pybind11_headers ALIAS pybind11_headers) # to match exported target
+  add_library(pybind11::headers ALIAS pybind11_headers) # easier to use/remember
 
-# Build the headers-only target (no Python included):
+  target_include_directories(
-# (long name used here to keep this from clashing in subdirectory mode)
+    pybind11_headers ${pybind11_system} INTERFACE $<BUILD_INTERFACE:${pybind11_INCLUDE_DIR}>
-add_library(pybind11_headers INTERFACE)
+                                                  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
-add_library(pybind11::pybind11_headers ALIAS pybind11_headers) # to match exported target
+
-add_library(pybind11::headers ALIAS pybind11_headers) # easier to use/remember
+  target_compile_features(pybind11_headers INTERFACE cxx_inheriting_constructors cxx_user_literals
+                                                     cxx_right_angle_brackets)
+  if(NOT "${PYBIND11_INTERNALS_VERSION}" STREQUAL "")
+    target_compile_definitions(
+      pybind11_headers INTERFACE "PYBIND11_INTERNALS_VERSION=${PYBIND11_INTERNALS_VERSION}")
+  endif()
+else()
+  # It is invalid to install a target twice, too.
+  set(PYBIND11_INSTALL OFF)
+endif()
 
 include("${CMAKE_CURRENT_SOURCE_DIR}/tools/pybind11Common.cmake")
 
@@ -168,21 +206,18 @@ elseif(USE_PYTHON_INCLUDE_DIR AND DEFINED PYTHON_INCLUDE_DIR)
   file(RELATIVE_PATH CMAKE_INSTALL_INCLUDEDIR ${CMAKE_INSTALL_PREFIX} ${PYTHON_INCLUDE_DIRS})
 endif()
 
-# Fill in headers target
-target_include_directories(
-  pybind11_headers ${pybind11_system} INTERFACE $<BUILD_INTERFACE:${PYBIND11_INCLUDE_DIR}>
-                                                $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
-
-target_compile_features(pybind11_headers INTERFACE cxx_inheriting_constructors cxx_user_literals
-                                                   cxx_right_angle_brackets)
-
 if(PYBIND11_INSTALL)
-  install(DIRECTORY ${PYBIND11_INCLUDE_DIR}/pybind11 DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+  install(DIRECTORY ${pybind11_INCLUDE_DIR}/pybind11 DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
-  # GNUInstallDirs "DATADIR" wrong here; CMake search path wants "share".
   set(PYBIND11_CMAKECONFIG_INSTALL_DIR
-      "share/cmake/${PROJECT_NAME}"
+      "${CMAKE_INSTALL_DATAROOTDIR}/cmake/${PROJECT_NAME}"
       CACHE STRING "install path for pybind11Config.cmake")
 
+  if(IS_ABSOLUTE "${CMAKE_INSTALL_INCLUDEDIR}")
+    set(pybind11_INCLUDEDIR "${CMAKE_INSTALL_FULL_INCLUDEDIR}")
+  else()
+    set(pybind11_INCLUDEDIR "\$\{PACKAGE_PREFIX_DIR\}/${CMAKE_INSTALL_INCLUDEDIR}")
+  endif()
+
   configure_package_config_file(
     tools/${PROJECT_NAME}Config.cmake.in "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
     INSTALL_DESTINATION ${PYBIND11_CMAKECONFIG_INSTALL_DIR})
@@ -260,8 +295,5 @@ endif()
 if(NOT PYBIND11_MASTER_PROJECT)
   set(pybind11_FOUND
       TRUE
-      CACHE INTERNAL "true if pybind11 and all required components found on the system")
+      CACHE INTERNAL "True if pybind11 and all required components found on the system")
-  set(pybind11_INCLUDE_DIR
-      "${PYBIND11_INCLUDE_DIR}"
-      CACHE INTERNAL "Directory where pybind11 headers are located")
 endif()

wrap/pybind11/MANIFEST.in

@@ -1,4 +1,6 @@
 recursive-include pybind11/include/pybind11 *.h
 recursive-include pybind11 *.py
+recursive-include pybind11 py.typed
+recursive-include pybind11 *.pyi
 include pybind11/share/cmake/pybind11/*.cmake
-include LICENSE README.md pyproject.toml setup.py setup.cfg
+include LICENSE README.rst pyproject.toml setup.py setup.cfg

wrap/pybind11/README.md

@@ -1,145 +0,0 @@
-![pybind11 logo](https://github.com/pybind/pybind11/raw/master/docs/pybind11-logo.png)
-
-# pybind11 — Seamless operability between C++11 and Python
-
-[![Documentation Status](https://readthedocs.org/projects/pybind11/badge/?version=master)](http://pybind11.readthedocs.org/en/master/?badge=master)
-[![Documentation Status](https://readthedocs.org/projects/pybind11/badge/?version=stable)](http://pybind11.readthedocs.org/en/stable/?badge=stable)
-[![Gitter chat](https://img.shields.io/gitter/room/gitterHQ/gitter.svg)](https://gitter.im/pybind/Lobby)
-[![CI](https://github.com/pybind/pybind11/workflows/CI/badge.svg)](https://github.com/pybind/pybind11/actions)
-[![Build status](https://ci.appveyor.com/api/projects/status/riaj54pn4h08xy40?svg=true)](https://ci.appveyor.com/project/wjakob/pybind11)
-
-**pybind11** is a lightweight header-only library that exposes C++ types in
-Python and vice versa, mainly to create Python bindings of existing C++ code.
-Its goals and syntax are similar to the excellent [Boost.Python][] library by
-David Abrahams: to minimize boilerplate code in traditional extension modules
-by inferring type information using compile-time introspection.
-
-The main issue with Boost.Python—and the reason for creating such a similar
-project—is Boost. Boost is an enormously large and complex suite of utility
-libraries that works with almost every C++ compiler in existence. This
-compatibility has its cost: arcane template tricks and workarounds are
-necessary to support the oldest and buggiest of compiler specimens. Now that
-C++11-compatible compilers are widely available, this heavy machinery has
-become an excessively large and unnecessary dependency.
-
-Think of this library as a tiny self-contained version of Boost.Python with
-everything stripped away that isn't relevant for binding generation. Without
-comments, the core header files only require ~4K lines of code and depend on
-Python (2.7 or 3.5+, or PyPy) and the C++ standard library. This compact
-implementation was possible thanks to some of the new C++11 language features
-(specifically: tuples, lambda functions and variadic templates). Since its
-creation, this library has grown beyond Boost.Python in many ways, leading to
-dramatically simpler binding code in many common situations.
-
-Tutorial and reference documentation is provided at
-[pybind11.readthedocs.org][].  A PDF version of the manual is available
-[here][docs-pdf].
-
-## Core features
-pybind11 can map the following core C++ features to Python:
-
-- Functions accepting and returning custom data structures per value, reference, or pointer
-- Instance methods and static methods
-- Overloaded functions
-- Instance attributes and static attributes
-- Arbitrary exception types
-- Enumerations
-- Callbacks
-- Iterators and ranges
-- Custom operators
-- Single and multiple inheritance
-- STL data structures
-- Smart pointers with reference counting like `std::shared_ptr`
-- Internal references with correct reference counting
-- C++ classes with virtual (and pure virtual) methods can be extended in Python
-
-## Goodies
-In addition to the core functionality, pybind11 provides some extra goodies:
-
-- Python 2.7, 3.5+, and PyPy (tested on 7.3) are supported with an implementation-agnostic
-  interface.
-
-- It is possible to bind C++11 lambda functions with captured variables. The
-  lambda capture data is stored inside the resulting Python function object.
-
-- pybind11 uses C++11 move constructors and move assignment operators whenever
-  possible to efficiently transfer custom data types.
-
-- It's easy to expose the internal storage of custom data types through
-  Pythons' buffer protocols. This is handy e.g. for fast conversion between
-  C++ matrix classes like Eigen and NumPy without expensive copy operations.
-
-- pybind11 can automatically vectorize functions so that they are transparently
-  applied to all entries of one or more NumPy array arguments.
-
-- Python's slice-based access and assignment operations can be supported with
-  just a few lines of code.
-
-- Everything is contained in just a few header files; there is no need to link
-  against any additional libraries.
-
-- Binaries are generally smaller by a factor of at least 2 compared to
-  equivalent bindings generated by Boost.Python. A recent pybind11 conversion
-  of PyRosetta, an enormous Boost.Python binding project,
-  [reported][pyrosetta-report] a binary size reduction of **5.4x** and compile
-  time reduction by **5.8x**.
-
-- Function signatures are precomputed at compile time (using `constexpr`),
-  leading to smaller binaries.
-
-- With little extra effort, C++ types can be pickled and unpickled similar to
-  regular Python objects.
-
-## Supported compilers
-
-1. Clang/LLVM 3.3 or newer (for Apple Xcode's clang, this is 5.0.0 or newer)
-2. GCC 4.8 or newer
-3. Microsoft Visual Studio 2015 Update 3 or newer
-4. Intel C++ compiler 17 or newer (16 with pybind11 v2.0 and 15 with pybind11
-   v2.0 and a [workaround][intel-15-workaround])
-5. Cygwin/GCC (tested on 2.5.1)
-6. NVCC (CUDA 11 tested)
-7. NVIDIA PGI (20.7 tested)
-
-## About
-
-This project was created by [Wenzel Jakob](http://rgl.epfl.ch/people/wjakob).
-Significant features and/or improvements to the code were contributed by
-Jonas Adler,
-Lori A. Burns,
-Sylvain Corlay,
-Trent Houliston,
-Axel Huebl,
-@hulucc,
-Sergey Lyskov
-Johan Mabille,
-Tomasz Miąsko,
-Dean Moldovan,
-Ben Pritchard,
-Jason Rhinelander,
-Boris Schäling,
-Pim Schellart,
-Henry Schreiner,
-Ivan Smirnov, and
-Patrick Stewart.
-
-### Contributing
-
-See the [contributing guide][] for information on building and contributing to
-pybind11.
-
-
-### License
-
-pybind11 is provided under a BSD-style license that can be found in the
-[`LICENSE`][] file. By using, distributing, or contributing to this project,
-you agree to the terms and conditions of this license.
-
-
-[pybind11.readthedocs.org]: http://pybind11.readthedocs.org/en/master
-[docs-pdf]: https://media.readthedocs.org/pdf/pybind11/master/pybind11.pdf
-[Boost.Python]: http://www.boost.org/doc/libs/1_58_0/libs/python/doc/
-[pyrosetta-report]: http://graylab.jhu.edu/RosettaCon2016/PyRosetta-4.pdf
-[contributing guide]:  https://github.com/pybind/pybind11/blob/master/.github/CONTRIBUTING.md
-[`LICENSE`]: https://github.com/pybind/pybind11/blob/master/LICENSE
-[intel-15-workaround]: https://github.com/pybind/pybind11/issues/276

wrap/pybind11/README.rst

@@ -0,0 +1,180 @@
+.. figure:: https://github.com/pybind/pybind11/raw/master/docs/pybind11-logo.png
+   :alt: pybind11 logo
+
+**pybind11 — Seamless operability between C++11 and Python**
+
+|Latest Documentation Status| |Stable Documentation Status| |Gitter chat| |GitHub Discussions| |CI| |Build status|
+
+|Repology| |PyPI package| |Conda-forge| |Python Versions|
+
+`Setuptools example <https://github.com/pybind/python_example>`_
+• `Scikit-build example <https://github.com/pybind/scikit_build_example>`_
+• `CMake example <https://github.com/pybind/cmake_example>`_
+
+.. start
+
+
+**pybind11** is a lightweight header-only library that exposes C++ types
+in Python and vice versa, mainly to create Python bindings of existing
+C++ code. Its goals and syntax are similar to the excellent
+`Boost.Python <http://www.boost.org/doc/libs/1_58_0/libs/python/doc/>`_
+library by David Abrahams: to minimize boilerplate code in traditional
+extension modules by inferring type information using compile-time
+introspection.
+
+The main issue with Boost.Python—and the reason for creating such a
+similar project—is Boost. Boost is an enormously large and complex suite
+of utility libraries that works with almost every C++ compiler in
+existence. This compatibility has its cost: arcane template tricks and
+workarounds are necessary to support the oldest and buggiest of compiler
+specimens. Now that C++11-compatible compilers are widely available,
+this heavy machinery has become an excessively large and unnecessary
+dependency.
+
+Think of this library as a tiny self-contained version of Boost.Python
+with everything stripped away that isn’t relevant for binding
+generation. Without comments, the core header files only require ~4K
+lines of code and depend on Python (2.7 or 3.5+, or PyPy) and the C++
+standard library. This compact implementation was possible thanks to
+some of the new C++11 language features (specifically: tuples, lambda
+functions and variadic templates). Since its creation, this library has
+grown beyond Boost.Python in many ways, leading to dramatically simpler
+binding code in many common situations.
+
+Tutorial and reference documentation is provided at
+`pybind11.readthedocs.io <https://pybind11.readthedocs.io/en/latest>`_.
+A PDF version of the manual is available
+`here <https://pybind11.readthedocs.io/_/downloads/en/latest/pdf/>`_.
+And the source code is always available at
+`github.com/pybind/pybind11 <https://github.com/pybind/pybind11>`_.
+
+
+Core features
+-------------
+
+
+pybind11 can map the following core C++ features to Python:
+
+- Functions accepting and returning custom data structures per value,
+  reference, or pointer
+- Instance methods and static methods
+- Overloaded functions
+- Instance attributes and static attributes
+- Arbitrary exception types
+- Enumerations
+- Callbacks
+- Iterators and ranges
+- Custom operators
+- Single and multiple inheritance
+- STL data structures
+- Smart pointers with reference counting like ``std::shared_ptr``
+- Internal references with correct reference counting
+- C++ classes with virtual (and pure virtual) methods can be extended
+  in Python
+
+Goodies
+-------
+
+In addition to the core functionality, pybind11 provides some extra
+goodies:
+
+- Python 2.7, 3.5+, and PyPy/PyPy3 7.3 are supported with an
+  implementation-agnostic interface.
+
+- It is possible to bind C++11 lambda functions with captured
+  variables. The lambda capture data is stored inside the resulting
+  Python function object.
+
+- pybind11 uses C++11 move constructors and move assignment operators
+  whenever possible to efficiently transfer custom data types.
+
+- It’s easy to expose the internal storage of custom data types through
+  Pythons’ buffer protocols. This is handy e.g. for fast conversion
+  between C++ matrix classes like Eigen and NumPy without expensive
+  copy operations.
+
+- pybind11 can automatically vectorize functions so that they are
+  transparently applied to all entries of one or more NumPy array
+  arguments.
+
+- Python's slice-based access and assignment operations can be
+  supported with just a few lines of code.
+
+- Everything is contained in just a few header files; there is no need
+  to link against any additional libraries.
+
+- Binaries are generally smaller by a factor of at least 2 compared to
+  equivalent bindings generated by Boost.Python. A recent pybind11
+  conversion of PyRosetta, an enormous Boost.Python binding project,
+  `reported <https://graylab.jhu.edu/Sergey/2016.RosettaCon/PyRosetta-4.pdf>`_
+  a binary size reduction of **5.4x** and compile time reduction by
+  **5.8x**.
+
+- Function signatures are precomputed at compile time (using
+  ``constexpr``), leading to smaller binaries.
+
+- With little extra effort, C++ types can be pickled and unpickled
+  similar to regular Python objects.
+
+Supported compilers
+-------------------
+
+1. Clang/LLVM 3.3 or newer (for Apple Xcode’s clang, this is 5.0.0 or
+   newer)
+2. GCC 4.8 or newer
+3. Microsoft Visual Studio 2015 Update 3 or newer
+4. Intel classic C++ compiler 18 or newer (ICC 20.2 tested in CI)
+5. Cygwin/GCC (previously tested on 2.5.1)
+6. NVCC (CUDA 11.0 tested in CI)
+7. NVIDIA PGI (20.9 tested in CI)
+
+About
+-----
+
+This project was created by `Wenzel
+Jakob <http://rgl.epfl.ch/people/wjakob>`_. Significant features and/or
+improvements to the code were contributed by Jonas Adler, Lori A. Burns,
+Sylvain Corlay, Eric Cousineau, Aaron Gokaslan, Ralf Grosse-Kunstleve, Trent Houliston, Axel
+Huebl, @hulucc, Yannick Jadoul, Sergey Lyskov Johan Mabille, Tomasz Miąsko,
+Dean Moldovan, Ben Pritchard, Jason Rhinelander, Boris Schäling, Pim
+Schellart, Henry Schreiner, Ivan Smirnov, Boris Staletic, and Patrick Stewart.
+
+We thank Google for a generous financial contribution to the continuous
+integration infrastructure used by this project.
+
+
+Contributing
+~~~~~~~~~~~~
+
+See the `contributing
+guide <https://github.com/pybind/pybind11/blob/master/.github/CONTRIBUTING.md>`_
+for information on building and contributing to pybind11.
+
+License
+~~~~~~~
+
+pybind11 is provided under a BSD-style license that can be found in the
+`LICENSE <https://github.com/pybind/pybind11/blob/master/LICENSE>`_
+file. By using, distributing, or contributing to this project, you agree
+to the terms and conditions of this license.
+
+.. |Latest Documentation Status| image:: https://readthedocs.org/projects/pybind11/badge?version=latest
+   :target: http://pybind11.readthedocs.org/en/latest
+.. |Stable Documentation Status| image:: https://img.shields.io/badge/docs-stable-blue.svg
+   :target: http://pybind11.readthedocs.org/en/stable
+.. |Gitter chat| image:: https://img.shields.io/gitter/room/gitterHQ/gitter.svg
+   :target: https://gitter.im/pybind/Lobby
+.. |CI| image:: https://github.com/pybind/pybind11/workflows/CI/badge.svg
+   :target: https://github.com/pybind/pybind11/actions
+.. |Build status| image:: https://ci.appveyor.com/api/projects/status/riaj54pn4h08xy40?svg=true
+   :target: https://ci.appveyor.com/project/wjakob/pybind11
+.. |PyPI package| image:: https://img.shields.io/pypi/v/pybind11.svg
+   :target: https://pypi.org/project/pybind11/
+.. |Conda-forge| image:: https://img.shields.io/conda/vn/conda-forge/pybind11.svg
+   :target: https://github.com/conda-forge/pybind11-feedstock
+.. |Repology| image:: https://repology.org/badge/latest-versions/python:pybind11.svg
+   :target: https://repology.org/project/python:pybind11/versions
+.. |Python Versions| image:: https://img.shields.io/pypi/pyversions/pybind11.svg
+   :target: https://pypi.org/project/pybind11/
+.. |GitHub Discussions| image:: https://img.shields.io/static/v1?label=Discussions&message=Ask&color=blue&logo=github
+   :target: https://github.com/pybind/pybind11/discussions

wrap/pybind11/docs/Doxyfile

@@ -18,5 +18,5 @@ ALIASES               += "endrst=\endverbatim"
 QUIET                  = YES
 WARNINGS               = YES
 WARN_IF_UNDOCUMENTED   = NO
-PREDEFINED             = DOXYGEN_SHOULD_SKIP_THIS \
+PREDEFINED             = PY_MAJOR_VERSION=3 \
-                         PY_MAJOR_VERSION=3
+                         PYBIND11_NOINLINE

wrap/pybind11/docs/advanced/cast/custom.rst

@@ -26,7 +26,9 @@ The following Python snippet demonstrates the intended usage from the Python sid
         def __int__(self):
             return 123
 
+
     from example import print
+
     print(A())
 
 To register the necessary conversion routines, it is necessary to add an
@@ -44,7 +46,7 @@ type is explicitly allowed.
              * function signatures and declares a local variable
              * 'value' of type inty
              */
-            PYBIND11_TYPE_CASTER(inty, _("inty"));
+            PYBIND11_TYPE_CASTER(inty, const_name("inty"));
 
             /**
              * Conversion part 1 (Python->C++): convert a PyObject into a inty

wrap/pybind11/docs/advanced/cast/eigen.rst

@@ -52,7 +52,7 @@ can be mapped *and* if the numpy array is writeable (that is
 the passed variable will be transparently carried out directly on the
 ``numpy.ndarray``.
 
-This means you can can write code such as the following and have it work as
+This means you can write code such as the following and have it work as
 expected:
 
 .. code-block:: cpp
@@ -112,7 +112,7 @@ example:
 .. code-block:: python
 
     a = MyClass()
-    m = a.get_matrix()   # flags.writeable = True,  flags.owndata = False
+    m = a.get_matrix()  # flags.writeable = True,  flags.owndata = False
     v = a.view_matrix()  # flags.writeable = False, flags.owndata = False
     c = a.copy_matrix()  # flags.writeable = True,  flags.owndata = True
     # m[5,6] and v[5,6] refer to the same element, c[5,6] does not.
@@ -203,7 +203,7 @@ adding the ``order='F'`` option when creating an array:
 
 .. code-block:: python
 
-    myarray = np.array(source, order='F')
+    myarray = np.array(source, order="F")
 
 Such an object will be passable to a bound function accepting an
 ``Eigen::Ref<MatrixXd>`` (or similar column-major Eigen type).

wrap/pybind11/docs/advanced/cast/overview.rst

@@ -75,91 +75,97 @@ The following basic data types are supported out of the box (some may require
 an additional extension header to be included). To pass other data structures
 as arguments and return values, refer to the section on binding :ref:`classes`.
 
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-|  Data type                         |  Description              | Header file                   |
+|  Data type                         |  Description              | Header file                       |
-+====================================+===========================+===============================+
++====================================+===========================+===================================+
-| ``int8_t``, ``uint8_t``            | 8-bit integers            | :file:`pybind11/pybind11.h`   |
+| ``int8_t``, ``uint8_t``            | 8-bit integers            | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``int16_t``, ``uint16_t``          | 16-bit integers           | :file:`pybind11/pybind11.h`   |
+| ``int16_t``, ``uint16_t``          | 16-bit integers           | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``int32_t``, ``uint32_t``          | 32-bit integers           | :file:`pybind11/pybind11.h`   |
+| ``int32_t``, ``uint32_t``          | 32-bit integers           | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``int64_t``, ``uint64_t``          | 64-bit integers           | :file:`pybind11/pybind11.h`   |
+| ``int64_t``, ``uint64_t``          | 64-bit integers           | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``ssize_t``, ``size_t``            | Platform-dependent size   | :file:`pybind11/pybind11.h`   |
+| ``ssize_t``, ``size_t``            | Platform-dependent size   | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``float``, ``double``              | Floating point types      | :file:`pybind11/pybind11.h`   |
+| ``float``, ``double``              | Floating point types      | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``bool``                           | Two-state Boolean type    | :file:`pybind11/pybind11.h`   |
+| ``bool``                           | Two-state Boolean type    | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``char``                           | Character literal         | :file:`pybind11/pybind11.h`   |
+| ``char``                           | Character literal         | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``char16_t``                       | UTF-16 character literal  | :file:`pybind11/pybind11.h`   |
+| ``char16_t``                       | UTF-16 character literal  | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``char32_t``                       | UTF-32 character literal  | :file:`pybind11/pybind11.h`   |
+| ``char32_t``                       | UTF-32 character literal  | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``wchar_t``                        | Wide character literal    | :file:`pybind11/pybind11.h`   |
+| ``wchar_t``                        | Wide character literal    | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``const char *``                   | UTF-8 string literal      | :file:`pybind11/pybind11.h`   |
+| ``const char *``                   | UTF-8 string literal      | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``const char16_t *``               | UTF-16 string literal     | :file:`pybind11/pybind11.h`   |
+| ``const char16_t *``               | UTF-16 string literal     | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``const char32_t *``               | UTF-32 string literal     | :file:`pybind11/pybind11.h`   |
+| ``const char32_t *``               | UTF-32 string literal     | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``const wchar_t *``                | Wide string literal       | :file:`pybind11/pybind11.h`   |
+| ``const wchar_t *``                | Wide string literal       | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::string``                    | STL dynamic UTF-8 string  | :file:`pybind11/pybind11.h`   |
+| ``std::string``                    | STL dynamic UTF-8 string  | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::u16string``                 | STL dynamic UTF-16 string | :file:`pybind11/pybind11.h`   |
+| ``std::u16string``                 | STL dynamic UTF-16 string | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::u32string``                 | STL dynamic UTF-32 string | :file:`pybind11/pybind11.h`   |
+| ``std::u32string``                 | STL dynamic UTF-32 string | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::wstring``                   | STL dynamic wide string   | :file:`pybind11/pybind11.h`   |
+| ``std::wstring``                   | STL dynamic wide string   | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::string_view``,              | STL C++17 string views    | :file:`pybind11/pybind11.h`   |
+| ``std::string_view``,              | STL C++17 string views    | :file:`pybind11/pybind11.h`       |
-| ``std::u16string_view``, etc.      |                           |                               |
+| ``std::u16string_view``, etc.      |                           |                                   |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::pair<T1, T2>``              | Pair of two custom types  | :file:`pybind11/pybind11.h`   |
+| ``std::pair<T1, T2>``              | Pair of two custom types  | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::tuple<...>``                | Arbitrary tuple of types  | :file:`pybind11/pybind11.h`   |
+| ``std::tuple<...>``                | Arbitrary tuple of types  | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::reference_wrapper<...>``    | Reference type wrapper    | :file:`pybind11/pybind11.h`   |
+| ``std::reference_wrapper<...>``    | Reference type wrapper    | :file:`pybind11/pybind11.h`       |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::complex<T>``                | Complex numbers           | :file:`pybind11/complex.h`    |
+| ``std::complex<T>``                | Complex numbers           | :file:`pybind11/complex.h`        |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::array<T, Size>``            | STL static array          | :file:`pybind11/stl.h`        |
+| ``std::array<T, Size>``            | STL static array          | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::vector<T>``                 | STL dynamic array         | :file:`pybind11/stl.h`        |
+| ``std::vector<T>``                 | STL dynamic array         | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::deque<T>``                  | STL double-ended queue    | :file:`pybind11/stl.h`        |
+| ``std::deque<T>``                  | STL double-ended queue    | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::valarray<T>``               | STL value array           | :file:`pybind11/stl.h`        |
+| ``std::valarray<T>``               | STL value array           | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::list<T>``                   | STL linked list           | :file:`pybind11/stl.h`        |
+| ``std::list<T>``                   | STL linked list           | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::map<T1, T2>``               | STL ordered map           | :file:`pybind11/stl.h`        |
+| ``std::map<T1, T2>``               | STL ordered map           | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::unordered_map<T1, T2>``     | STL unordered map         | :file:`pybind11/stl.h`        |
+| ``std::unordered_map<T1, T2>``     | STL unordered map         | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::set<T>``                    | STL ordered set           | :file:`pybind11/stl.h`        |
+| ``std::set<T>``                    | STL ordered set           | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::unordered_set<T>``          | STL unordered set         | :file:`pybind11/stl.h`        |
+| ``std::unordered_set<T>``          | STL unordered set         | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::optional<T>``               | STL optional type (C++17) | :file:`pybind11/stl.h`        |
+| ``std::optional<T>``               | STL optional type (C++17) | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::experimental::optional<T>`` | STL optional type (exp.)  | :file:`pybind11/stl.h`        |
+| ``std::experimental::optional<T>`` | STL optional type (exp.)  | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::variant<...>``              | Type-safe union (C++17)   | :file:`pybind11/stl.h`        |
+| ``std::variant<...>``              | Type-safe union (C++17)   | :file:`pybind11/stl.h`            |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::function<...>``             | STL polymorphic function  | :file:`pybind11/functional.h` |
+| ``std::filesystem::path<T>``       | STL path (C++17) [#]_     | :file:`pybind11/stl/filesystem.h` |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::chrono::duration<...>``     | STL time duration         | :file:`pybind11/chrono.h`     |
+| ``std::function<...>``             | STL polymorphic function  | :file:`pybind11/functional.h`     |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``std::chrono::time_point<...>``   | STL date/time             | :file:`pybind11/chrono.h`     |
+| ``std::chrono::duration<...>``     | STL time duration         | :file:`pybind11/chrono.h`         |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``Eigen::Matrix<...>``             | Eigen: dense matrix       | :file:`pybind11/eigen.h`      |
+| ``std::chrono::time_point<...>``   | STL date/time             | :file:`pybind11/chrono.h`         |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``Eigen::Map<...>``                | Eigen: mapped memory      | :file:`pybind11/eigen.h`      |
+| ``Eigen::Matrix<...>``             | Eigen: dense matrix       | :file:`pybind11/eigen.h`          |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
-| ``Eigen::SparseMatrix<...>``       | Eigen: sparse matrix      | :file:`pybind11/eigen.h`      |
+| ``Eigen::Map<...>``                | Eigen: mapped memory      | :file:`pybind11/eigen.h`          |
-+------------------------------------+---------------------------+-------------------------------+
++------------------------------------+---------------------------+-----------------------------------+
+| ``Eigen::SparseMatrix<...>``       | Eigen: sparse matrix      | :file:`pybind11/eigen.h`          |
++------------------------------------+---------------------------+-----------------------------------+
+
+.. [#] ``std::filesystem::path`` is converted to ``pathlib.Path`` and
+   ``os.PathLike`` is converted to ``std::filesystem::path``, but this requires
+   Python 3.6 (for ``__fspath__`` support).

wrap/pybind11/docs/advanced/cast/stl.rst

@@ -5,7 +5,7 @@ Automatic conversion
 ====================
 
 When including the additional header file :file:`pybind11/stl.h`, conversions
-between ``std::vector<>``/``std::deque<>``/``std::list<>``/``std::array<>``,
+between ``std::vector<>``/``std::deque<>``/``std::list<>``/``std::array<>``/``std::valarray<>``,
 ``std::set<>``/``std::unordered_set<>``, and
 ``std::map<>``/``std::unordered_map<>`` and the Python ``list``, ``set`` and
 ``dict`` data structures are automatically enabled. The types ``std::pair<>``
@@ -72,6 +72,17 @@ The ``visit_helper`` specialization is not required if your ``name::variant`` pr
 a ``name::visit()`` function. For any other function name, the specialization must be
 included to tell pybind11 how to visit the variant.
 
+.. warning::
+
+    When converting a ``variant`` type, pybind11 follows the same rules as when
+    determining which function overload to call (:ref:`overload_resolution`), and
+    so the same caveats hold. In particular, the order in which the ``variant``'s
+    alternatives are listed is important, since pybind11 will try conversions in
+    this order. This means that, for example, when converting ``variant<int, bool>``,
+    the ``bool`` variant will never be selected, as any Python ``bool`` is already
+    an ``int`` and is convertible to a C++ ``int``. Changing the order of alternatives
+    (and using ``variant<bool, int>``, in this example) provides a solution.
+
 .. note::
 
     pybind11 only supports the modern implementation of ``boost::variant``

wrap/pybind11/docs/advanced/cast/strings.rst

@@ -36,13 +36,13 @@ everywhere <http://utf8everywhere.org/>`_.
         }
     );
 
-.. code-block:: python
+.. code-block:: pycon
 
-    >>> utf8_test('🎂')
+    >>> utf8_test("🎂")
     utf-8 is icing on the cake.
     🎂
 
-    >>> utf8_charptr('🍕')
+    >>> utf8_charptr("🍕")
     My favorite food is
     🍕
 
@@ -80,7 +80,7 @@ raise a ``UnicodeDecodeError``.
         }
     );
 
-.. code-block:: python
+.. code-block:: pycon
 
     >>> isinstance(example.std_string_return(), str)
     True
@@ -114,7 +114,7 @@ conversion has the same overhead as implicit conversion.
         }
     );
 
-.. code-block:: python
+.. code-block:: pycon
 
     >>> str_output()
     'Send your résumé to Alice in HR'
@@ -143,7 +143,7 @@ returned to Python as ``bytes``, then one can return the data as a
         }
     );
 
-.. code-block:: python
+.. code-block:: pycon
 
     >>> example.return_bytes()
     b'\xba\xd0\xba\xd0'
@@ -160,7 +160,7 @@ encoding, but cannot convert ``std::string`` back to ``bytes`` implicitly.
         }
     );
 
-.. code-block:: python
+.. code-block:: pycon
 
     >>> isinstance(example.asymmetry(b"have some bytes"), str)
     True
@@ -229,16 +229,16 @@ character.
     m.def("pass_char", [](char c) { return c; });
     m.def("pass_wchar", [](wchar_t w) { return w; });
 
-.. code-block:: python
+.. code-block:: pycon
 
-    >>> example.pass_char('A')
+    >>> example.pass_char("A")
     'A'
 
 While C++ will cast integers to character types (``char c = 0x65;``), pybind11
 does not convert Python integers to characters implicitly. The Python function
 ``chr()`` can be used to convert integers to characters.
 
-.. code-block:: python
+.. code-block:: pycon
 
     >>> example.pass_char(0x65)
     TypeError
@@ -259,17 +259,17 @@ a combining acute accent). The combining character will be lost if the
 two-character sequence is passed as an argument, even though it renders as a
 single grapheme.
 
-.. code-block:: python
+.. code-block:: pycon
 
-    >>> example.pass_wchar('é')
+    >>> example.pass_wchar("é")
     'é'
 
-    >>> combining_e_acute = 'e' + '\u0301'
+    >>> combining_e_acute = "e" + "\u0301"
 
     >>> combining_e_acute
     'é'
 
-    >>> combining_e_acute == 'é'
+    >>> combining_e_acute == "é"
     False
 
     >>> example.pass_wchar(combining_e_acute)
@@ -278,9 +278,9 @@ single grapheme.
 Normalizing combining characters before passing the character literal to C++
 may resolve *some* of these issues:
 
-.. code-block:: python
+.. code-block:: pycon
 
-    >>> example.pass_wchar(unicodedata.normalize('NFC', combining_e_acute))
+    >>> example.pass_wchar(unicodedata.normalize("NFC", combining_e_acute))
     'é'
 
 In some languages (Thai for example), there are `graphemes that cannot be

wrap/pybind11/docs/advanced/classes.rst

@@ -9,7 +9,7 @@ that you are already familiar with the basics from :doc:`/classes`.
 Overriding virtual functions in Python
 ======================================
 
-Suppose that a C++ class or interface has a virtual function that we'd like to
+Suppose that a C++ class or interface has a virtual function that we'd like
 to override from within Python (we'll focus on the class ``Animal``; ``Dog`` is
 given as a specific example of how one would do this with traditional C++
 code).
@@ -136,7 +136,7 @@ a virtual method call.
     u'woof! woof! woof! '
     >>> class Cat(Animal):
     ...     def go(self, n_times):
-    ...             return "meow! " * n_times
+    ...         return "meow! " * n_times
     ...
     >>> c = Cat()
     >>> call_go(c)
@@ -159,8 +159,9 @@ Here is an example:
 
     class Dachshund(Dog):
         def __init__(self, name):
-            Dog.__init__(self) # Without this, a TypeError is raised.
+            Dog.__init__(self)  # Without this, a TypeError is raised.
             self.name = name
+
         def bark(self):
             return "yap!"
 
@@ -259,7 +260,7 @@ override the ``name()`` method):
 
 .. note::
 
-    Note the trailing commas in the ``PYBIND11_OVERIDE`` calls to ``name()``
+    Note the trailing commas in the ``PYBIND11_OVERRIDE`` calls to ``name()``
     and ``bark()``. These are needed to portably implement a trampoline for a
     function that does not take any arguments. For functions that take
     a nonzero number of arguments, the trailing comma must be omitted.
@@ -804,7 +805,7 @@ to bind these two functions:
             }
         ));
 
-The ``__setstate__`` part of the ``py::picke()`` definition follows the same
+The ``__setstate__`` part of the ``py::pickle()`` definition follows the same
 rules as the single-argument version of ``py::init()``. The return type can be
 a value, pointer or holder type. See :ref:`custom_constructors` for details.
 
@@ -1153,12 +1154,65 @@ error:
 
     >>> class PyFinalChild(IsFinal):
     ...     pass
+    ...
     TypeError: type 'IsFinal' is not an acceptable base type
 
 .. note:: This attribute is currently ignored on PyPy
 
 .. versionadded:: 2.6
 
+Binding classes with template parameters
+========================================
+
+pybind11 can also wrap classes that have template parameters. Consider these classes:
+
+.. code-block:: cpp
+
+    struct Cat {};
+    struct Dog {};
+
+    template <typename PetType>
+    struct Cage {
+        Cage(PetType& pet);
+        PetType& get();
+    };
+
+C++ templates may only be instantiated at compile time, so pybind11 can only
+wrap instantiated templated classes. You cannot wrap a non-instantiated template:
+
+.. code-block:: cpp
+
+    // BROKEN (this will not compile)
+    py::class_<Cage>(m, "Cage");
+        .def("get", &Cage::get);
+
+You must explicitly specify each template/type combination that you want to
+wrap separately.
+
+.. code-block:: cpp
+
+    // ok
+    py::class_<Cage<Cat>>(m, "CatCage")
+        .def("get", &Cage<Cat>::get);
+
+    // ok
+    py::class_<Cage<Dog>>(m, "DogCage")
+        .def("get", &Cage<Dog>::get);
+
+If your class methods have template parameters you can wrap those as well,
+but once again each instantiation must be explicitly specified:
+
+.. code-block:: cpp
+
+    typename <typename T>
+    struct MyClass {
+        template <typename V>
+        T fn(V v);
+    };
+
+    py::class<MyClass<int>>(m, "MyClassT")
+        .def("fn", &MyClass<int>::fn<std::string>);
+
 Custom automatic downcasters
 ============================
 
@@ -1247,7 +1301,7 @@ Accessing the type object
 
 You can get the type object from a C++ class that has already been registered using:
 
-.. code-block:: python
+.. code-block:: cpp
 
     py::type T_py = py::type::of<T>();
 
@@ -1259,3 +1313,37 @@ object, just like ``type(ob)`` in Python.
     Other types, like ``py::type::of<int>()``, do not work, see :ref:`type-conversions`.
 
 .. versionadded:: 2.6
+
+Custom type setup
+=================
+
+For advanced use cases, such as enabling garbage collection support, you may
+wish to directly manipulate the ``PyHeapTypeObject`` corresponding to a
+``py::class_`` definition.
+
+You can do that using ``py::custom_type_setup``:
+
+.. code-block:: cpp
+
+   struct OwnsPythonObjects {
+       py::object value = py::none();
+   };
+   py::class_<OwnsPythonObjects> cls(
+       m, "OwnsPythonObjects", py::custom_type_setup([](PyHeapTypeObject *heap_type) {
+           auto *type = &heap_type->ht_type;
+           type->tp_flags |= Py_TPFLAGS_HAVE_GC;
+           type->tp_traverse = [](PyObject *self_base, visitproc visit, void *arg) {
+               auto &self = py::cast<OwnsPythonObjects&>(py::handle(self_base));
+               Py_VISIT(self.value.ptr());
+               return 0;
+           };
+           type->tp_clear = [](PyObject *self_base) {
+               auto &self = py::cast<OwnsPythonObjects&>(py::handle(self_base));
+               self.value = py::none();
+               return 0;
+           };
+       }));
+   cls.def(py::init<>());
+   cls.def_readwrite("value", &OwnsPythonObjects::value);
+
+.. versionadded:: 2.8