diff --git a/docs/source/nnls_tutorial.rst b/docs/source/nnls_tutorial.rst
index 3a2cc65..8882cad 100644
--- a/docs/source/nnls_tutorial.rst
+++ b/docs/source/nnls_tutorial.rst
@@ -913,36 +913,125 @@
    The executable :member:`pose_graph_2d` expects the first argument to be
    the path to the problem definition. To run the executable,
 
-  .. code-block:: bash
+   .. code-block:: bash
 
-     /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o
+      /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o
 
-  A python script is provided to visualize the resulting output files.
+   A python script is provided to visualize the resulting output files.
 
-  .. code-block:: bash
+   .. code-block:: bash
 
-     /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+      /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
 
-  As an example, a standard synthetic benchmark dataset [#f10]_ created by Edwin
-  Olson which has 3500 nodes in a grid world with a total of 5598 edges was
-  solved.  Visualizing the results with the provided script produces:
+   As an example, a standard synthetic benchmark dataset [#f10]_ created by
+   Edwin Olson which has 3500 nodes in a grid world with a total of 5598 edges
+   was solved.  Visualizing the results with the provided script produces:
 
-  .. figure:: manhattan_olson_3500_result.png
-     :figwidth: 600px
-     :height: 600px
-     :align: center
+   .. figure:: manhattan_olson_3500_result.png
+      :figwidth: 600px
+      :height: 600px
+      :align: center
 
-  with the original poses in green and the optimized poses in blue. As shown,
-  the optimized poses more closely match the underlying grid world. Note, the
-  left side of the graph has a small yaw drift due to a lack of relative
-  constraints to provide enough information to reconstruct the trajectory.
+   with the original poses in green and the optimized poses in blue. As shown,
+   the optimized poses more closely match the underlying grid world. Note, the
+   left side of the graph has a small yaw drift due to a lack of relative
+   constraints to provide enough information to reconstruct the trajectory.
 
-  .. rubric:: Footnotes
+   .. rubric:: Footnotes
 
-  .. [#f9] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram
-     Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation
-     Systems Magazine, 52(3):199–222, 2010.
+   .. [#f9] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram
+      Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation
+      Systems Magazine, 52(3):199–222, 2010.
 
-  .. [#f10] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of
-     pose graphs with poor initial estimates,” in Robotics and Automation
-     (ICRA), IEEE International Conference on, 2006, pp. 2262–2269.
+   .. [#f10] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of
+      pose graphs with poor initial estimates,” in Robotics and Automation
+      (ICRA), IEEE International Conference on, 2006, pp. 2262–2269.
+
+#. `slam/pose_graph_3d/pose_graph_3d.cc
+   <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/pose_graph_3d.cc>`_
+   The following explains how to formulate the pose graph based SLAM problem in
+   3-Dimensions with relative pose constraints. The example also illustrates how
+   to use Eigen's geometry module with Ceres's automatic differentiation
+   functionality. 
+
+   The robot at timestamp :math:`t` has state :math:`x_t = [p^T, q^T]^T` where
+   :math:`p` is a 3D vector that represents the position and :math:`q` is the
+   orientation represented as an Eigen quaternion. The measurement of the
+   relative transform between the robot state at two timestamps :math:`a` and
+   :math:`b` is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{q}_{ab}^T]^T`.
+   The residual implemented in the Ceres cost function which computes the error
+   between the measurement and the predicted measurement is:
+
+   .. math:: r_{ab} =
+             \left[
+             \begin{array}{c}
+                R(q_a)^{T} (p_b - p_a) - \hat{p}_{ab} \\
+                2.0 \mathrm{vec}\left((q_a^{-1} q_b) \hat{q}_{ab}^{-1}\right)
+             \end{array}
+             \right]
+
+   where the function :math:`\mathrm{vec}()` returns the vector part of the
+   quaternion, i.e. :math:`[q_x, q_y, q_z]`, and :math:`R(q)` is the rotation
+   matrix for the quaternion.
+
+   To finish the cost function, we need to weight the residual by the
+   uncertainty of the measurement. Hence, we pre-multiply the residual by the
+   inverse square root of the covariance matrix for the measurement,
+   i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is
+   the covariance.
+
+   Given that we are using a quaternion to represent the orientation, we need to
+   use a local parameterization (:class:`EigenQuaternionParameterization`) to
+   only apply updates orthogonal to the 4-vector defining the
+   quaternion. Eigen's quaternion uses a different internal memory layout for
+   the elements of the quaternion than what is commonly used. Specifically,
+   Eigen stores the elements in memory as :math:`[x, y, z, w]` where the real
+   part is last whereas it is typically stored first. Note, when creating an
+   Eigen quaternion through the constructor the elements are accepted in
+   :math:`w`, :math:`x`, :math:`y`, :math:`z` order. Since Ceres operates on
+   parameter blocks which are raw double pointers this difference is important
+   and requires a different parameterization.
+
+   This package includes an executable :member:`pose_graph_3d` that will read a
+   problem definition file. This executable can work with any 3D problem
+   definition that uses the g2o format with quaternions used for the orientation
+   representation. It would be relatively straightforward to implement a new
+   reader for a different format such as TORO or others. :member:`pose_graph_3d`
+   will print the Ceres solver full summary and then output to disk the original
+   and optimized poses (``poses_original.txt`` and ``poses_optimized.txt``,
+   respectively) of the robot in the following format:
+
+   .. code-block:: bash
+
+      pose_id x y z q_x q_y q_z q_w
+      pose_id x y z q_x q_y q_z q_w
+      pose_id x y z q_x q_y q_z q_w
+      ...
+
+   where ``pose_id`` is the corresponding integer ID from the file
+   definition. Note, the file will be sorted in ascending order for the
+   ``pose_id``.
+
+   The executable :member:`pose_graph_3d` expects the first argument to be the
+   path to the problem definition. The executable can be run via
+
+   .. code-block:: bash
+
+      /path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o
+
+   A script is provided to visualize the resulting output files. There is also
+   an option to enable equal axes using ``--axes_equal``
+
+   .. code-block:: bash
+
+      /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+
+   As an example, a standard synthetic benchmark dataset [#f9]_ where the robot is
+   traveling on the surface of a sphere which has 2500 nodes with a total of
+   4949 edges was solved. Visualizing the results with the provided script
+   produces:
+
+   .. figure:: pose_graph_3d_ex.png
+      :figwidth: 600px
+      :height: 300px
+      :align: center
diff --git a/docs/source/pose_graph_3d_ex.png b/docs/source/pose_graph_3d_ex.png
new file mode 100644
index 0000000..ae2cfc3
--- /dev/null
+++ b/docs/source/pose_graph_3d_ex.png
Binary files differ
diff --git a/examples/slam/CMakeLists.txt b/examples/slam/CMakeLists.txt
index d2c0d42..a5f12c4 100644
--- a/examples/slam/CMakeLists.txt
+++ b/examples/slam/CMakeLists.txt
@@ -27,4 +27,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 # Author: vitus@google.com (Michael Vitus)
+include_directories(./)
+
 add_subdirectory(pose_graph_2d)
+
+if (GFLAGS)
+  add_subdirectory(pose_graph_3d)
+endif (GFLAGS)
\ No newline at end of file
diff --git a/examples/slam/common/read_g2o.h b/examples/slam/common/read_g2o.h
new file mode 100644
index 0000000..71b071c
--- /dev/null
+++ b/examples/slam/common/read_g2o.h
@@ -0,0 +1,141 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Reads a file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+                std::map<int, Pose, std::less<int>, Allocator>* poses) {
+  int id;
+  Pose pose;
+  *infile >> id >> pose;
+
+  // Ensure we don't have duplicate poses.
+  if (poses->find(id) != poses->end()) {
+    LOG(ERROR) << "Duplicate vertex with ID: " << id;
+    return false;
+  }
+  (*poses)[id] = pose;
+
+  return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+                    std::vector<Constraint, Allocator>* constraints) {
+  Constraint constraint;
+  *infile >> constraint;
+
+  constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, a vertex is defined as follows:
+//
+// VERTEX_SE2 ID x_meters y_meters yaw_radians
+//
+// A constraint is defined as follows:
+//
+// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose, typename Constraint, typename MapAllocator,
+          typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+                 std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+                 std::vector<Constraint, VectorAllocator>* constraints) {
+  CHECK(poses != NULL);
+  CHECK(constraints != NULL);
+
+  poses->clear();
+  constraints->clear();
+
+  std::ifstream infile(filename.c_str());
+  if (!infile) {
+    return false;
+  }
+
+  std::string data_type;
+  while (infile.good()) {
+    // Read whether the type is a node or a constraint.
+    infile >> data_type;
+    if (data_type == Pose::name()) {
+      if (!ReadVertex(&infile, poses)) {
+        return false;
+      }
+    } else if (data_type == Constraint::name()) {
+      ReadConstraint(&infile, constraints);
+    } else {
+      LOG(ERROR) << "Unknown data type: " << data_type;
+      return false;
+    }
+
+    // Clear any trailing whitespace from the line.
+    infile >> std::ws;
+  }
+
+  return true;
+}
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // EXAMPLES_CERES_READ_G2O_H_
diff --git a/examples/slam/pose_graph_2d/CMakeLists.txt b/examples/slam/pose_graph_2d/CMakeLists.txt
index 117f4cd..5574ad1 100644
--- a/examples/slam/pose_graph_2d/CMakeLists.txt
+++ b/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -33,7 +33,6 @@
   normalize_angle.h
   pose_graph_2d.cc
   pose_graph_2d_error_term.h
-  read_g2o.cc
-  )
+  types.h)
 target_link_libraries(pose_graph_2d ceres)
 
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d.cc b/examples/slam/pose_graph_2d/pose_graph_2d.cc
index e6caf7e..969bdb5 100644
--- a/examples/slam/pose_graph_2d/pose_graph_2d.cc
+++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -41,8 +41,8 @@
 
 #include "angle_local_parameterization.h"
 #include "ceres/ceres.h"
+#include "common/read_g2o.h"
 #include "pose_graph_2d_error_term.h"
-#include "read_g2o.h"
 #include "types.h"
 
 namespace ceres {
diff --git a/examples/slam/pose_graph_2d/read_g2o.cc b/examples/slam/pose_graph_2d/read_g2o.cc
deleted file mode 100644
index 22f424b..0000000
--- a/examples/slam/pose_graph_2d/read_g2o.cc
+++ /dev/null
@@ -1,122 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
-// http://ceres-solver.org/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-//   this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-//   this list of conditions and the following disclaimer in the documentation
-//   and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-//   used to endorse or promote products derived from this software without
-//   specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: vitus@google.com (Michael Vitus)
-
-#include "read_g2o.h"
-
-#include <iostream>
-#include <fstream>
-
-#include "Eigen/Core"
-#include "glog/logging.h"
-#include "normalize_angle.h"
-
-namespace ceres {
-namespace examples {
-namespace {
-// Reads a single pose from the input and inserts it into the map. Returns false
-// if there is a duplicate entry.
-bool ReadVertex(std::ifstream* infile, std::map<int, Pose2d>* poses) {
-  int id;
-  Pose2d pose;
-  *infile >> id >> pose.x >> pose.y >> pose.yaw_radians;
-  // Normalize the angle between -pi to pi.
-  pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
-  // Ensure we don't have duplicate poses.
-  if (poses->find(id) != poses->end()) {
-    std::cerr << "Duplicate vertex with ID: " << id << '\n';
-    return false;
-  }
-  (*poses)[id] = pose;
-  return true;
-}
-
-// Reads the contraints between two vertices in the pose graph
-void ReadConstraint(std::ifstream* infile,
-                    std::vector<Constraint2d>* constraints) {
-  Constraint2d constraint;
-
-  // Read in the constraint data which is the x, y, yaw_radians and then the
-  // upper triangular part of the information matrix.
-  *infile >> constraint.id_begin >> constraint.id_end >> constraint.x >>
-      constraint.y >> constraint.yaw_radians >>
-      constraint.information(0, 0) >> constraint.information(0, 1) >>
-      constraint.information(0, 2) >> constraint.information(1, 1) >>
-      constraint.information(1, 2) >> constraint.information(2, 2);
-
-  // Set the lower triangular part of the information matrix.
-  constraint.information(1, 0) = constraint.information(0, 1);
-  constraint.information(2, 0) = constraint.information(0, 2);
-  constraint.information(2, 1) = constraint.information(1, 2);
-
-  // Normalize the angle between -pi to pi.
-  constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
-
-  constraints->push_back(constraint);
-}
-}
-
-bool ReadG2oFile(const std::string& filename, std::map<int, Pose2d>* poses,
-                 std::vector<Constraint2d>* constraints) {
-  CHECK(poses != NULL);
-  CHECK(constraints != NULL);
-
-  poses->clear();
-  constraints->clear();
-
-  std::ifstream infile(filename.c_str());
-  if (!infile) {
-    std::cerr << "Error reading the file: " << filename << '\n';
-    return false;
-  }
-
-  std::string data_type;
-  while (infile.good()) {
-    // Read whether the type is a node or a constraint.
-    infile >> data_type;
-    if (data_type == "VERTEX_SE2") {
-      if (!ReadVertex(&infile, poses)) {
-        return false;
-      }
-    } else if (data_type == "EDGE_SE2") {
-      ReadConstraint(&infile, constraints);
-    } else {
-      std::cerr << "Unknown data type: " << data_type << '\n';
-      return false;
-    }
-
-    // Clear any trailing whitespace from the file.
-    infile >> std::ws;
-  }
-
-  return true;
-}
-
-}  // namespace examples
-}  // namespace ceres
diff --git a/examples/slam/pose_graph_2d/read_g2o.h b/examples/slam/pose_graph_2d/read_g2o.h
deleted file mode 100644
index dfa30d6..0000000
--- a/examples/slam/pose_graph_2d/read_g2o.h
+++ /dev/null
@@ -1,64 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
-// http://ceres-solver.org/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-//   this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-//   this list of conditions and the following disclaimer in the documentation
-//   and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-//   used to endorse or promote products derived from this software without
-//   specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: vitus@google.com (Michael Vitus)
-//
-// Reads a 2D pose graph problem formulation in g2o format.
-
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_
-
-#include <string>
-#include <map>
-#include <vector>
-
-#include "types.h"
-
-namespace ceres {
-namespace examples {
-
-// Reads a file in the g2o filename format that describes a 2D pose graph
-// problem. The g2o format consists of two entries, vertices and constraints. A
-// vertex is defined as follows:
-//
-// VERTEX_SE2 ID x_meters y_meters yaw_radians
-//
-// A constraint is defined as follows:
-//
-// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
-//
-// where I_ij is the (i, j)-th entry of the information matrix for the
-// measurement.
-bool ReadG2oFile(const std::string& filename,
-                 std::map<int, Pose2d>* poses,
-                 std::vector<Constraint2d>* constraints);
-
-}  // namespace examples
-}  // namespace ceres
-
-#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_
diff --git a/examples/slam/pose_graph_2d/types.h b/examples/slam/pose_graph_2d/types.h
index d54c733..a54d9bf 100644
--- a/examples/slam/pose_graph_2d/types.h
+++ b/examples/slam/pose_graph_2d/types.h
@@ -35,7 +35,10 @@
 #ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
 #define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
 
+#include <fstream>
+
 #include "Eigen/Core"
+#include "normalize_angle.h"
 
 namespace ceres {
 namespace examples {
@@ -45,8 +48,20 @@
   double x;
   double y;
   double yaw_radians;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() {
+    return "VERTEX_SE2";
+  }
 };
 
+std::istream& operator>>(std::istream& input, Pose2d& pose) {
+  input >> pose.x >> pose.y >> pose.yaw_radians;
+  // Normalize the angle between -pi to pi.
+  pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+  return input;
+}
+
 // The constraint between two vertices in the pose graph. The constraint is the
 // transformation from vertex id_begin to vertex id_end.
 struct Constraint2d {
@@ -60,8 +75,30 @@
   // The inverse of the covariance matrix for the measurement. The order of the
   // entries are x, y, and yaw.
   Eigen::Matrix3d information;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() {
+    return "EDGE_SE2";
+  }
 };
 
+std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+  input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+      constraint.y >> constraint.yaw_radians >>
+      constraint.information(0, 0) >> constraint.information(0, 1) >>
+      constraint.information(0, 2) >> constraint.information(1, 1) >>
+      constraint.information(1, 2) >> constraint.information(2, 2);
+
+  // Set the lower triangular part of the information matrix.
+  constraint.information(1, 0) = constraint.information(0, 1);
+  constraint.information(2, 0) = constraint.information(0, 2);
+  constraint.information(2, 1) = constraint.information(1, 2);
+
+  // Normalize the angle between -pi to pi.
+  constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+  return input;
+}
+
 }  // namespace examples
 }  // namespace ceres
 
diff --git a/examples/slam/pose_graph_3d/CMakeLists.txt b/examples/slam/pose_graph_3d/CMakeLists.txt
new file mode 100644
index 0000000..1be4e85
--- /dev/null
+++ b/examples/slam/pose_graph_3d/CMakeLists.txt
@@ -0,0 +1,32 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2016 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: vitus@google.com (Michael Vitus)
+
+add_executable(pose_graph_3d pose_graph_3d.cc)
+target_link_libraries(pose_graph_3d ceres ${GFLAGS_LIBRARIES})
diff --git a/examples/slam/pose_graph_3d/README.md b/examples/slam/pose_graph_3d/README.md
new file mode 100644
index 0000000..a16aa09
--- /dev/null
+++ b/examples/slam/pose_graph_3d/README.md
@@ -0,0 +1,54 @@
+Pose Graph 3D
+----------------
+
+The Simultaneous Localization and Mapping (SLAM) problem consists of building a
+map of an unknown environment while simultaneously localizing against this
+map. The main difficulty of this problem stems from not having any additional
+external aiding information such as GPS. SLAM has been considered one of the
+fundamental challenges of robotics. A pose graph optimization problem is one
+example of a SLAM problem.
+
+The example also illustrates how to use Eigen's geometry module with Ceres'
+automatic differentiation functionality. To represent the orientation, we will
+use Eigen's quaternion which uses the Hamiltonian convention but has different
+element ordering as compared with Ceres's rotation representation. Specifically
+they differ by whether the scalar component q_w is first or last; the element
+order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
+is [q_x, q_y, q_z, q_w].
+
+This package defines the necessary Ceres cost functions needed to model the
+3-dimensional pose graph optimization problem as well as a binary to build and
+solve the problem. The cost functions are shown for instruction purposes and can
+be speed up by using analytical derivatives which take longer to implement.
+
+
+Running
+-----------
+This package includes an executable `pose_graph_3d` that will read a problem
+definition file. This executable can work with any 3D problem definition that
+uses the g2o format with quaternions used for the orientation representation. It
+would be relatively straightforward to implement a new reader for a different
+format such as TORO or others. `pose_graph_3d` will print the Ceres solver full
+summary and then output to disk the original and optimized poses
+(`poses_original.txt` and `poses_optimized.txt`, respectively) of the robot in
+the following format:
+```
+pose_id x y z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+...
+```
+where `pose_id` is the corresponding integer ID from the file definition. Note,
+the file will be sorted in ascending order for the ```pose_id```.
+
+The executable `pose_graph_3d` expects the first argument to be the path to the
+problem definition. To run the executable,
+```
+/path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o
+```
+
+A script is provided to visualize the resulting output files. There is also an
+option to enable equal axes using ```--axes_equal```.
+```
+/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+```
diff --git a/examples/slam/pose_graph_3d/plot_results.py b/examples/slam/pose_graph_3d/plot_results.py
new file mode 100755
index 0000000..defb9e6
--- /dev/null
+++ b/examples/slam/pose_graph_3d/plot_results.py
@@ -0,0 +1,80 @@
+#!/usr/bin/python
+#
+# Plots the results from the 3D pose graph optimization. It will draw a line
+# between consecutive vertices.  The commandline expects two optional filenames:
+#
+#   ./plot_results.py --initial_poses filename  --optimized_poses filename
+#
+# The files have the following format:
+#   ID x y z q_x q_y q_z q_w
+
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plot
+import numpy
+import sys
+from optparse import OptionParser
+
+def set_axes_equal(axes):
+    ''' Sets the axes of a 3D plot to have equal scale. '''
+    x_limits = axes.get_xlim3d()
+    y_limits = axes.get_ylim3d()
+    z_limits = axes.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = numpy.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = numpy.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = numpy.mean(z_limits)
+
+    length = 0.5 * max([x_range, y_range, z_range])
+
+    axes.set_xlim3d([x_middle - length, x_middle + length])
+    axes.set_ylim3d([y_middle - length, y_middle + length])
+    axes.set_zlim3d([z_middle - length, z_middle + length])
+
+parser = OptionParser()
+parser.add_option("--initial_poses", dest="initial_poses",
+                  default="", help="The filename that contains the original poses.")
+parser.add_option("--optimized_poses", dest="optimized_poses",
+                  default="", help="The filename that contains the optimized poses.")
+parser.add_option("-e", "--axes_equal", action="store_true", dest="axes_equal",
+                  default="", help="Make the plot axes equal.")
+(options, args) = parser.parse_args()
+
+# Read the original and optimized poses files.
+poses_original = None
+if options.initial_poses != '':
+  poses_original = numpy.genfromtxt(options.initial_poses,
+                                    usecols = (1, 2, 3))
+
+poses_optimized = None
+if options.optimized_poses != '':
+  poses_optimized = numpy.genfromtxt(options.optimized_poses,
+                                     usecols = (1, 2, 3))
+
+# Plots the results for the specified poses.
+figure = plot.figure()
+
+if poses_original is not None:
+  axes = plot.subplot(1, 2, 1, projection='3d')
+  plot.plot(poses_original[:, 0], poses_original[:, 1], poses_original[:, 2],
+            '-', alpha=0.5, color="green")
+  plot.title('Original')
+  if options.axes_equal:
+    axes.set_aspect('equal')
+    set_axes_equal(axes)
+
+
+if poses_optimized is not None:
+  axes = plot.subplot(1, 2, 2, projection='3d')
+  plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], poses_optimized[:, 2],
+            '-', alpha=0.5, color="blue")
+  plot.title('Optimized')
+  if options.axes_equal:
+    axes.set_aspect('equal')
+    set_axes_equal(plot.gca())
+
+
+# Show the plot and wait for the user to close.
+plot.show()
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d.cc b/examples/slam/pose_graph_3d/pose_graph_3d.cc
new file mode 100644
index 0000000..c7c47fd
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -0,0 +1,175 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#include <iostream>
+#include <fstream>
+#include <string>
+
+#include "ceres/ceres.h"
+#include "common/read_g2o.h"
+#include "glog/logging.h"
+#include "pose_graph_3d_error_term.h"
+#include "types.h"
+
+DEFINE_string(input_filename, "",
+              "The pose graph definition filename in g2o format.");
+
+namespace ceres {
+namespace examples {
+
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
+void BuildOptimizationProblem(const VectorOfConstraints& constraints,
+                              MapOfPoses* poses, ceres::Problem* problem) {
+  CHECK(poses != NULL);
+  CHECK(problem != NULL);
+  if (constraints.empty()) {
+    LOG(INFO) << "No constraints, no problem to optimize.";
+    return;
+  }
+
+  ceres::LossFunction* loss_function = NULL;
+  ceres::LocalParameterization* quaternion_local_parameterization =
+      new EigenQuaternionParameterization;
+
+  for (VectorOfConstraints::const_iterator constraints_iter =
+           constraints.begin();
+       constraints_iter != constraints.end(); ++constraints_iter) {
+    const Constraint3d& constraint = *constraints_iter;
+
+    MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
+    CHECK(pose_begin_iter != poses->end())
+        << "Pose with ID: " << constraint.id_begin << " not found.";
+    MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
+    CHECK(pose_end_iter != poses->end())
+        << "Pose with ID: " << constraint.id_end << " not found.";
+
+    const Eigen::Matrix<double, 6, 6> sqrt_information =
+        constraint.information.llt().matrixL();
+    // Ceres will take ownership of the pointer.
+    ceres::CostFunction* cost_function =
+        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
+
+    problem->AddResidualBlock(cost_function, loss_function,
+                              pose_begin_iter->second.p.data(),
+                              pose_begin_iter->second.q.coeffs().data(),
+                              pose_end_iter->second.p.data(),
+                              pose_end_iter->second.q.coeffs().data());
+
+    problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
+                                 quaternion_local_parameterization);
+    problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
+                                 quaternion_local_parameterization);
+  }
+
+  // The pose graph optimization problem has six DOFs that are not fully
+  // constrained. This is typically referred to as gauge freedom. You can apply
+  // a rigid body transformation to all the nodes and the optimization problem
+  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
+  // internal damping which mitigates this issue, but it is better to properly
+  // constrain the gauge freedom. This can be done by setting one of the poses
+  // as constant so the optimizer cannot change it.
+  MapOfPoses::iterator pose_start_iter = poses->begin();
+  CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+  problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
+  problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
+}
+
+// Returns true if the solve was successful.
+bool SolveOptimizationProblem(ceres::Problem* problem) {
+  CHECK(problem != NULL);
+
+  ceres::Solver::Options options;
+  options.max_num_iterations = 200;
+  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, problem, &summary);
+
+  std::cout << summary.FullReport() << '\n';
+
+  return summary.IsSolutionUsable();
+}
+
+// Output the poses to the file with format: id x y z q_x q_y q_z q_w.
+bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
+  std::fstream outfile;
+  outfile.open(filename.c_str(), std::istream::out);
+  if (!outfile) {
+    LOG(ERROR) << "Error opening the file: " << filename;
+    return false;
+  }
+  for (std::map<int, Pose3d, std::less<int>,
+                Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+           const_iterator poses_iter = poses.begin();
+       poses_iter != poses.end(); ++poses_iter) {
+    const std::map<int, Pose3d, std::less<int>,
+                   Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+        value_type& pair = *poses_iter;
+    outfile << pair.first << " " << pair.second.p.transpose() << " "
+            << pair.second.q.x() << " " << pair.second.q.y() << " "
+            << pair.second.q.z() << " " << pair.second.q.w() << '\n';
+  }
+  return true;
+}
+
+}  // namespace examples
+}  // namespace ceres
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(FLAGS_input_filename != "") << "Need to specify the filename to read.";
+
+  ceres::examples::MapOfPoses poses;
+  ceres::examples::VectorOfConstraints constraints;
+
+  CHECK(
+      ceres::examples::ReadG2oFile(FLAGS_input_filename, &poses, &constraints))
+      << "Error reading the file: " << FLAGS_input_filename;
+
+  std::cout << "Number of poses: " << poses.size() << '\n';
+  std::cout << "Number of constraints: " << constraints.size() << '\n';
+
+  CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
+      << "Error outputting to poses_original.txt";
+
+  ceres::Problem problem;
+  ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
+
+  CHECK(ceres::examples::SolveOptimizationProblem(&problem))
+      << "The solve was not successful, exiting.";
+
+  CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
+      << "Error outputting to poses_original.txt";
+
+  return 0;
+}
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
new file mode 100644
index 0000000..aca819e
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
@@ -0,0 +1,131 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+
+#include "types.h"
+
+namespace ceres {
+namespace examples {
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement. We have two poses x_a
+// and x_b. Through sensor measurements we can measure the transformation of
+// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
+// between the current estimate of the poses and the measurement.
+//
+// In this formulation, we have chosen to represent the rigid transformation as
+// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
+// [x, y, z, w].
+
+// The estimated measurement is:
+//      t_ab = [ p_ab ]  = [ R(q_a)^T * (p_b - p_a) ]
+//             [ q_ab ]    [ q_a^{-1] * q_b         ]
+//
+// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
+// quaternion. Now we can compute an error metric between the estimated and
+// measurement transformation. For the orientation error, we will use the
+// standard multiplicative error resulting in:
+//
+//   error = [ p_ab - \hat{p}_ab                 ]
+//           [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
+//
+// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
+// the measurement has an uncertainty associated with how accurate it is, we
+// will weight the errors by the square root of the measurement information
+// matrix:
+//
+//   residuals = I^{1/2) * error
+// where I is the information matrix which is the inverse of the covariance.
+class PoseGraph3dErrorTerm {
+ public:
+  PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
+                       const Eigen::Matrix<double, 6, 6>& sqrt_information)
+      : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
+                  const T* const p_b_ptr, const T* const q_b_ptr,
+                  T* residuals_ptr) const {
+    Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
+    Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
+
+    Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
+    Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
+
+    // Compute the relative transformation between the two frames.
+    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+    // Represent the displacement between the two frames in the A frame.
+    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+    // Compute the error between the two orientation estimates.
+    Eigen::Quaternion<T> delta_q =
+        t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
+
+    // Compute the residuals.
+    // [ position         ]   [ delta_p          ]
+    // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
+    Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
+    residuals.template block<3, 1>(0, 0) =
+        p_ab_estimated - t_ab_measured_.p.template cast<T>();
+    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+    // Scale the residuals by the measurement uncertainty.
+    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(
+      const Pose3d& t_ab_measured,
+      const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+    return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
+        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  // The measurement for the position of B relative to A in the A frame.
+  const Pose3d t_ab_measured_;
+  // The square root of the measurement information matrix.
+  const Eigen::Matrix<double, 6, 6> sqrt_information_;
+};
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/examples/slam/pose_graph_3d/types.h b/examples/slam/pose_graph_3d/types.h
new file mode 100644
index 0000000..2f12501
--- /dev/null
+++ b/examples/slam/pose_graph_3d/types.h
@@ -0,0 +1,114 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef EXAMPLES_CERES_TYPES_H_
+#define EXAMPLES_CERES_TYPES_H_
+
+#include <istream>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+
+namespace ceres {
+namespace examples {
+
+struct Pose3d {
+  Eigen::Vector3d p;
+  Eigen::Quaterniond q;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() {
+    return "VERTEX_SE3:QUAT";
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Pose3d& pose) {
+  input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>
+      pose.q.y() >> pose.q.z() >> pose.q.w();
+  // Normalize the quaternion to account for precision loss due to
+  // serialization.
+  pose.q.normalize();
+  return input;
+}
+
+typedef std::map<int, Pose3d, std::less<int>,
+                 Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
+    MapOfPoses;
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint3d {
+  int id_begin;
+  int id_end;
+
+  // The transformation that represents the pose of the end frame E w.r.t. the
+  // begin frame B. In other words, it transforms a vector in the E frame to
+  // the B frame.
+  Pose3d t_be;
+
+  // The inverse of the covariance matrix for the measurement. The order of the
+  // entries are x, y, z, delta orientation.
+  Eigen::Matrix<double, 6, 6> information;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() {
+    return "EDGE_SE3:QUAT";
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
+  Pose3d& t_be = constraint.t_be;
+  input >> constraint.id_begin >> constraint.id_end >> t_be;
+
+  for (int i = 0; i < 6 && input.good(); ++i) {
+    for (int j = i; j < 6 && input.good(); ++j) {
+      input >> constraint.information(i, j);
+      if (i != j) {
+        constraint.information(j, i) = constraint.information(i, j);
+      }
+    }
+  }
+  return input;
+}
+
+typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d> >
+    VectorOfConstraints;
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // EXAMPLES_CERES_TYPES_H_