diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 1a5a1ed96..2b9c5a45a 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -560,6 +560,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
 #endif
   };
 
+  /// std::vector of Rot3s, used in Matlab wrapper
+  using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3>>;
+
   /**
    * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
    * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index c28d8dc56..4d265fca2 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -28,7 +28,41 @@ class Point2 {
   // enabling serialization functionality
   void serialize() const;
 };
-  
+
+// Used in Matlab wrapper
+class Point2Pairs {
+  Point2Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point2Pair at(size_t n) const;
+  void push_back(const gtsam::Point2Pair& point_pair);
+};
+
+// std::vector<gtsam::Point2>
+// Used in Matlab wrapper
+class Point2Vector {
+  // Constructors
+  Point2Vector();
+  Point2Vector(const gtsam::Point2Vector& v);
+
+  // Capacity
+  size_t size() const;
+  size_t max_size() const;
+  void resize(size_t sz);
+  size_t capacity() const;
+  bool empty() const;
+  void reserve(size_t n);
+
+  // Element access
+  gtsam::Point2 at(size_t n) const;
+  gtsam::Point2 front() const;
+  gtsam::Point2 back() const;
+
+  // Modifiers
+  void assign(size_t n, const gtsam::Point2& u);
+  void push_back(const gtsam::Point2& x);
+  void pop_back();
+};
 
 #include <gtsam/geometry/StereoPoint2.h>
 class StereoPoint2 {
@@ -99,6 +133,14 @@ class Point3 {
   gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref<Eigen::MatrixXd> H) const;
 };
 
+// Used in Matlab wrapper
+class Point3Pairs {
+  Point3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point3Pair at(size_t n) const;
+  void push_back(const gtsam::Point3Pair& point_pair);
+};
 
 #include <gtsam/geometry/Rot2.h>
 class Rot2 {
@@ -503,6 +545,23 @@ class Pose3 {
   void serialize() const;
 };
 
+// Used in Matlab wrapper
+class Pose3Pairs {
+  Pose3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3Pair at(size_t n) const;
+  void push_back(const gtsam::Pose3Pair& pose_pair);
+};
+
+// Used in Matlab wrapper
+class Pose3Vector {
+  Pose3Vector();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3 at(size_t n) const;
+  void push_back(const gtsam::Pose3& pose);
+};
 
 #include <gtsam/geometry/Unit3.h>
 class Unit3 {
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 3e18168e3..834d5a147 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -104,6 +104,41 @@ class KeyGroupMap {
   bool insert2(size_t key, int val);
 };
 
+// Actually a FastSet<FactorIndex>
+// Used in Matlab wrapper
+class FactorIndexSet {
+  FactorIndexSet();
+  FactorIndexSet(const gtsam::FactorIndexSet& set);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(size_t factorIndex);
+  bool erase(size_t factorIndex);        // returns true if value was removed
+  bool count(size_t factorIndex) const;  // returns true if value exists
+};
+
+// Actually a vector<FactorIndex>
+// Used in Matlab wrapper
+class FactorIndices {
+  FactorIndices();
+  FactorIndices(const gtsam::FactorIndices& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  size_t at(size_t i) const;
+  size_t front() const;
+  size_t back() const;
+  void push_back(size_t factorIndex) const;
+};
+
 //*************************************************************************
 // Utilities
 //*************************************************************************
diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp
index 52f49574c..a6415df20 100644
--- a/gtsam/nonlinear/CustomFactor.cpp
+++ b/gtsam/nonlinear/CustomFactor.cpp
@@ -28,7 +28,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c
     if(H) {
       /*
        * In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
-       * As the type `std::vector<Matrix>` has been marked as opaque in `preamble/base.h`, any changes in
+       * As the type `std::vector<Matrix>` has been marked as opaque in `preamble/custom.h`, any changes in
        * Python will be immediately reflected on the C++ side.
        *
        * Example:
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 059aa21ed..2198692e8 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -99,6 +99,30 @@ typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
 typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
 
+// Used in Matlab wrapper
+class BinaryMeasurementsUnit3 {
+  BinaryMeasurementsUnit3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
+};
+
+// Used in Matlab wrapper
+class BinaryMeasurementsPoint3 {
+  BinaryMeasurementsPoint3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+};
+
+// Used in Matlab wrapper
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
+};
+
 #include <gtsam/slam/dataset.h>
 #include <gtsam/sfm/ShonanAveraging.h>
 
@@ -246,6 +270,16 @@ class ShonanAveraging3 {
 
 #include <gtsam/sfm/MFAS.h>
 
+// Used in Matlab wrapper
+class KeyPairDoubleMap {
+  KeyPairDoubleMap();
+  KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  size_t at(const pair<size_t, size_t>& keypair) const;
+};
 
 class MFAS {
   MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index dfdd63aa3..d35a87eee 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -250,6 +250,25 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
             const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
             string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
+// Used in Matlab wrapper
+class BetweenFactorPose2s {
+  BetweenFactorPose2s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
+};
+gtsam::BetweenFactorPose2s parse2DFactors(string filename);
+
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+// Used in Matlab wrapper
+class BetweenFactorPose3s {
+  BetweenFactorPose3s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
+};
+
 gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
@@ -288,7 +307,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
-gtsam::Rot3 FindKarcherMean(const std::vector<gtsam::Rot3>& rotations);
+gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
 
 #include <gtsam/slam/FrobeniusFactor.h>
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,