diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index 234f800..74ecf75 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -41,7 +41,7 @@ namespace mapping {
 
 PoseGraph2D::PoseGraph2D(
     const proto::PoseGraphOptions& options,
-    std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
+    std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
     common::ThreadPool* thread_pool)
     : options_(options),
       optimization_problem_(std::move(optimization_problem)),
@@ -250,8 +250,9 @@ void PoseGraph2D::ComputeConstraintsForNode(
       local_pose_2d;
   optimization_problem_->AddTrajectoryNode(
       matching_id.trajectory_id,
-      pose_graph::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d,
-                             constant_data->gravity_alignment});
+      optimization::NodeSpec2D{constant_data->time, local_pose_2d,
+                               global_pose_2d,
+                               constant_data->gravity_alignment});
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
     // Even if this was the last node added to 'submap_id', the submap will
@@ -446,8 +447,8 @@ void PoseGraph2D::AddSubmapFromProto(
   submap_data_.Insert(submap_id, InternalSubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
-  global_submap_poses_.Insert(submap_id,
-                              pose_graph::SubmapSpec2D{global_submap_pose_2d});
+  global_submap_poses_.Insert(
+      submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
   AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
     submap_data_.at(submap_id).state = SubmapState::kFinished;
     optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
@@ -471,7 +472,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
         constant_data->gravity_alignment.inverse());
     optimization_problem_->InsertTrajectoryNode(
         node_id,
-        pose_graph::NodeSpec2D{
+        optimization::NodeSpec2D{
             constant_data->time,
             transform::Project2D(constant_data->local_pose *
                                  gravity_alignment_inverse),
@@ -746,7 +747,7 @@ PoseGraph2D::GetAllSubmapPoses() {
 }
 
 transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
-    const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses,
+    const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
     const int trajectory_id) const {
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
   auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
index a5ee8ad..971683e 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.h
@@ -34,7 +34,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
-#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
 #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
@@ -61,7 +61,7 @@ class PoseGraph2D : public PoseGraph {
  public:
   PoseGraph2D(
       const proto::PoseGraphOptions& options,
-      std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
+      std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
       common::ThreadPool* thread_pool);
   ~PoseGraph2D() override;
 
@@ -201,7 +201,7 @@ class PoseGraph2D : public PoseGraph {
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses,
+      const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
       int trajectory_id) const REQUIRES(mutex_);
 
   SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_);
@@ -239,7 +239,7 @@ class PoseGraph2D : public PoseGraph {
   void DispatchOptimization() REQUIRES(mutex_);
 
   // Current optimization problem.
-  std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem_;
+  std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
   pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
   std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
 
@@ -252,7 +252,7 @@ class PoseGraph2D : public PoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Global submap poses currently used for displaying data.
-  MapById<SubmapId, pose_graph::SubmapSpec2D> global_submap_poses_
+  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_
       GUARDED_BY(mutex_);
 
   // Global landmark poses with all observations.
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc
index d6b5a56..3c60f55 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc
@@ -137,7 +137,7 @@ class PoseGraph2DTest : public ::testing::Test {
       auto options = CreatePoseGraphOptions(parameter_dictionary.get());
       pose_graph_ = common::make_unique<PoseGraph2D>(
           options,
-          common::make_unique<pose_graph::OptimizationProblem2D>(
+          common::make_unique<optimization::OptimizationProblem2D>(
               options.optimization_problem_options()),
           &thread_pool_);
     }
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index a19e446..ce9eaa9 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -41,7 +41,7 @@ namespace mapping {
 
 PoseGraph3D::PoseGraph3D(
     const proto::PoseGraphOptions& options,
-    std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
+    std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
     common::ThreadPool* thread_pool)
     : options_(options),
       optimization_problem_(std::move(optimization_problem)),
@@ -267,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
       insertion_submaps.front()->local_pose().inverse() * local_pose;
   optimization_problem_->AddTrajectoryNode(
       matching_id.trajectory_id,
-      pose_graph::NodeSpec3D{constant_data->time, local_pose, global_pose});
+      optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
     // Even if this was the last node added to 'submap_id', the submap will only
@@ -461,7 +461,7 @@ void PoseGraph3D::AddSubmapFromProto(
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
   global_submap_poses_.Insert(submap_id,
-                              pose_graph::SubmapSpec3D{global_submap_pose});
+                              optimization::SubmapSpec3D{global_submap_pose});
   AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
     submap_data_.at(submap_id).state = SubmapState::kFinished;
     optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
@@ -483,8 +483,8 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
     const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
     optimization_problem_->InsertTrajectoryNode(
         node_id,
-        pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose,
-                               global_pose});
+        optimization::NodeSpec3D{constant_data->time, constant_data->local_pose,
+                                 global_pose});
   });
 }
 
@@ -778,7 +778,7 @@ PoseGraph3D::GetAllSubmapPoses() {
 }
 
 transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
-    const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses,
+    const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses,
     const int trajectory_id) const {
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
   auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
index f0cba44..cf51dd9 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.h
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.h
@@ -34,7 +34,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/3d/submap_3d.h"
 #include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
-#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
 #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
@@ -60,7 +60,7 @@ class PoseGraph3D : public PoseGraph {
  public:
   PoseGraph3D(
       const proto::PoseGraphOptions& options,
-      std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
+      std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
       common::ThreadPool* thread_pool);
   ~PoseGraph3D() override;
 
@@ -200,7 +200,7 @@ class PoseGraph3D : public PoseGraph {
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses,
+      const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses,
       int trajectory_id) const REQUIRES(mutex_);
 
   PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@@ -243,7 +243,7 @@ class PoseGraph3D : public PoseGraph {
   void DispatchOptimization() REQUIRES(mutex_);
 
   // Current optimization problem.
-  std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem_;
+  std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
   pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
   std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
 
@@ -256,7 +256,7 @@ class PoseGraph3D : public PoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Global submap poses currently used for displaying data.
-  MapById<SubmapId, pose_graph::SubmapSpec3D> global_submap_poses_
+  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_
       GUARDED_BY(mutex_);
 
   // Global landmark poses with all observations.
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc
index a2ea75e..6f1b10b 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc
@@ -28,8 +28,8 @@ namespace cartographer {
 namespace mapping {
 namespace {
 
-using ::cartographer::mapping::pose_graph::OptimizationProblem3D;
-using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions;
+using ::cartographer::mapping::optimization::OptimizationProblem3D;
+using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions;
 using ::cartographer::transform::Rigid3d;
 
 class MockOptimizationProblem3D : public OptimizationProblem3D {
@@ -46,7 +46,7 @@ class PoseGraph3DForTesting : public PoseGraph3D {
  public:
   PoseGraph3DForTesting(
       const proto::PoseGraphOptions& options,
-      std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
+      std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
       common::ThreadPool* thread_pool)
       : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
 
@@ -68,7 +68,7 @@ class PoseGraph3DTest : public ::testing::Test {
 
   void BuildPoseGraph() {
     auto optimization_problem =
-        common::make_unique<pose_graph::OptimizationProblem3D>(
+        common::make_unique<optimization::OptimizationProblem3D>(
             pose_graph_options_.optimization_problem_options());
     pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
         pose_graph_options_, std::move(optimization_problem),
diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc
index cc889f6..ea8892c 100644
--- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc
+++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc
@@ -26,7 +26,7 @@
 #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h"
-#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/ceres_pose.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -76,7 +76,7 @@ void CeresScanMatcher3D::Match(
     transform::Rigid3d* const pose_estimate,
     ceres::Solver::Summary* const summary) {
   ceres::Problem problem;
-  pose_graph::CeresPose ceres_pose(
+  optimization::CeresPose ceres_pose(
       initial_pose_estimate, nullptr /* translation_parameterization */,
       options_.only_optimize_yaw()
           ? std::unique_ptr<ceres::LocalParameterization>(
diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.cc b/cartographer/mapping/internal/optimization/ceres_pose.cc
similarity index 93%
rename from cartographer/mapping/internal/pose_graph/ceres_pose.cc
rename to cartographer/mapping/internal/optimization/ceres_pose.cc
index 7063a47..4045fa5 100644
--- a/cartographer/mapping/internal/pose_graph/ceres_pose.cc
+++ b/cartographer/mapping/internal/optimization/ceres_pose.cc
@@ -14,11 +14,11 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/ceres_pose.h"
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 CeresPose::CeresPose(
     const transform::Rigid3d& rigid,
@@ -40,6 +40,6 @@ const transform::Rigid3d CeresPose::ToRigid() const {
   return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
 }
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.h b/cartographer/mapping/internal/optimization/ceres_pose.h
similarity index 86%
rename from cartographer/mapping/internal/pose_graph/ceres_pose.h
rename to cartographer/mapping/internal/optimization/ceres_pose.h
index fdd6d2e..9e617b6 100644
--- a/cartographer/mapping/internal/pose_graph/ceres_pose.h
+++ b/cartographer/mapping/internal/optimization/ceres_pose.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
 
 #include <array>
 #include <memory>
@@ -26,7 +26,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 class CeresPose {
  public:
@@ -53,8 +53,8 @@ class CeresPose {
   std::shared_ptr<Data> data_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
diff --git a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h
similarity index 93%
rename from cartographer/mapping/internal/3d/acceleration_cost_function_3d.h
rename to cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h
index b90293d..20325f1 100644
--- a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
@@ -101,4 +101,4 @@ class AccelerationCostFunction3D {
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers.h b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h
similarity index 89%
rename from cartographer/mapping/internal/pose_graph/cost_helpers.h
rename to cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h
index 9329d4a..8b54e29 100644
--- a/cartographer/mapping/internal/pose_graph/cost_helpers.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
@@ -24,7 +24,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 // Computes the error between the given relative pose and the difference of
 // poses 'start' and 'end' which are both in an arbitrary common frame.
@@ -80,10 +80,10 @@ InterpolateNodes2D(const T* const prev_node_pose,
                    const Eigen::Quaterniond& next_node_gravity_alignment,
                    const double interpolation_parameter);
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#include "cartographer/mapping/internal/pose_graph/cost_helpers_impl.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h"
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h
similarity index 96%
rename from cartographer/mapping/internal/pose_graph/cost_helpers_impl.h
rename to cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h
index eee01cf..6f20889 100644
--- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
 
 #include "Eigen/Core"
 #include "cartographer/transform/rigid_transform.h"
@@ -23,7 +23,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 template <typename T>
 static std::array<T, 3> ComputeUnscaledError(
@@ -190,8 +190,8 @@ InterpolateNodes2D(const T* const prev_node_pose,
            T(0)}});
 }
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h
similarity index 88%
rename from cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h
rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h
index 1c6a47f..d5e0719 100644
--- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h
@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h"
-#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -29,7 +29,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 // Cost function measuring the weighted error between the observed pose given by
 // the landmark measurement and the linearly interpolated pose of embedded in 3D
@@ -91,8 +91,8 @@ class LandmarkCostFunction2D {
   const double interpolation_parameter_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc
similarity index 94%
rename from cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc
rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc
index 9a0e505..e5d4c2a 100644
--- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc
+++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc
@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
 
 #include <memory>
 
@@ -24,7 +24,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 namespace {
 
 using ::testing::DoubleEq;
@@ -72,6 +72,6 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
 }
 
 }  // namespace
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h
similarity index 87%
rename from cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h
rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h
index b871e48..4d297e1 100644
--- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h
@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
-#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -29,7 +29,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 // Cost function measuring the weighted error between the observed pose given by
 // the landmark measurement and the linearly interpolated pose.
@@ -92,8 +92,8 @@ class LandmarkCostFunction3D {
   const double interpolation_parameter_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc
similarity index 94%
rename from cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc
rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc
index a270d3b..a5c620d 100644
--- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc
+++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc
@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h"
 
 #include <memory>
 
@@ -24,7 +24,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 namespace {
 
 using ::testing::DoubleEq;
@@ -72,6 +72,6 @@ TEST(LandmarkCostFunction3DTest, SmokeTest) {
 }
 
 }  // namespace
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h
similarity index 75%
rename from cartographer/mapping/internal/3d/rotation_cost_function_3d.h
rename to cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h
index cd61190..53390fc 100644
--- a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
@@ -25,15 +25,15 @@ namespace cartographer {
 namespace mapping {
 
 // Penalizes differences between IMU data and optimized orientations.
-class RotationCostFunction {
+class RotationCostFunction3D {
  public:
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const double scaling_factor,
       const Eigen::Quaterniond& delta_rotation_imu_frame) {
     return new ceres::AutoDiffCostFunction<
-        RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
+        RotationCostFunction3D, 3 /* residuals */, 4 /* rotation variables */,
         4 /* rotation variables */, 4 /* rotation variables */
-        >(new RotationCostFunction(scaling_factor, delta_rotation_imu_frame));
+        >(new RotationCostFunction3D(scaling_factor, delta_rotation_imu_frame));
   }
 
   template <typename T>
@@ -56,13 +56,13 @@ class RotationCostFunction {
   }
 
  private:
-  RotationCostFunction(const double scaling_factor,
-                       const Eigen::Quaterniond& delta_rotation_imu_frame)
+  RotationCostFunction3D(const double scaling_factor,
+                         const Eigen::Quaterniond& delta_rotation_imu_frame)
       : scaling_factor_(scaling_factor),
         delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
 
-  RotationCostFunction(const RotationCostFunction&) = delete;
-  RotationCostFunction& operator=(const RotationCostFunction&) = delete;
+  RotationCostFunction3D(const RotationCostFunction3D&) = delete;
+  RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete;
 
   const double scaling_factor_;
   const Eigen::Quaterniond delta_rotation_imu_frame_;
@@ -71,4 +71,4 @@ class RotationCostFunction {
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
diff --git a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h
similarity index 79%
rename from cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h
rename to cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h
index 97296a2..9020b47 100644
--- a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h
@@ -14,15 +14,15 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
 
 #include <array>
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
-#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -31,7 +31,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 class SpaCostFunction2D {
  public:
@@ -45,8 +45,8 @@ class SpaCostFunction2D {
 
   template <typename T>
   bool operator()(const T* const c_i, const T* const c_j, T* e) const {
-    using pose_graph::ComputeUnscaledError;
-    using pose_graph::ScaleError;
+    using optimization::ComputeUnscaledError;
+    using optimization::ScaleError;
 
     const std::array<T, 3> error = ScaleError(
         ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
@@ -62,8 +62,8 @@ class SpaCostFunction2D {
   const PoseGraph::Constraint::Pose pose_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
diff --git a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h
similarity index 83%
rename from cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h
rename to cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h
index d791bce..d4a400b 100644
--- a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h
+++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h
@@ -14,15 +14,15 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
 
 #include <array>
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
-#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -31,7 +31,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 class SpaCostFunction3D {
  public:
@@ -62,8 +62,8 @@ class SpaCostFunction3D {
   const PoseGraph::Constraint::Pose pose_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc
similarity index 96%
rename from cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
rename to cartographer/mapping/internal/optimization/optimization_problem_2d.cc
index 954e813..8a91277 100644
--- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
+++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc
@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
 
 #include <algorithm>
 #include <array>
@@ -27,10 +27,9 @@
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/histogram.h"
 #include "cartographer/common/math.h"
-#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h"
-#include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h"
-#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
-#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
+#include "cartographer/mapping/internal/optimization/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -38,10 +37,10 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 namespace {
 
-using ::cartographer::mapping::pose_graph::CeresPose;
+using ::cartographer::mapping::optimization::CeresPose;
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
 
 // Converts a pose into the 3 optimization variable format used for Ceres:
@@ -361,6 +360,6 @@ OptimizationProblem2D::CalculateOdometryBetweenNodes(
   return nullptr;
 }
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h
similarity index 88%
rename from cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
rename to cartographer/mapping/internal/optimization/optimization_problem_2d.h
index 5a56dd0..f5094d9 100644
--- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
+++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
 
 #include <array>
 #include <deque>
@@ -28,7 +28,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/id.h"
-#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h"
 #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/sensor/imu_data.h"
@@ -38,7 +38,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 struct NodeSpec2D {
   common::Time time;
@@ -56,7 +56,7 @@ class OptimizationProblem2D
                                           transform::Rigid2d> {
  public:
   explicit OptimizationProblem2D(
-      const pose_graph::proto::OptimizationProblemOptions& options);
+      const optimization::proto::OptimizationProblemOptions& options);
   ~OptimizationProblem2D();
 
   OptimizationProblem2D(const OptimizationProblem2D&) = delete;
@@ -108,7 +108,7 @@ class OptimizationProblem2D
       int trajectory_id, const NodeSpec2D& first_node_data,
       const NodeSpec2D& second_node_data) const;
 
-  pose_graph::proto::OptimizationProblemOptions options_;
+  optimization::proto::OptimizationProblemOptions options_;
   MapById<NodeId, NodeSpec2D> node_data_;
   MapById<SubmapId, SubmapSpec2D> submap_data_;
   std::map<std::string, transform::Rigid3d> landmark_data_;
@@ -116,8 +116,8 @@ class OptimizationProblem2D
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc
similarity index 97%
rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
rename to cartographer/mapping/internal/optimization/optimization_problem_3d.cc
index baa9089..4bd40f9 100644
--- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
+++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc
@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
 
 #include <algorithm>
 #include <array>
@@ -30,13 +30,13 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/internal/3d/acceleration_cost_function_3d.h"
 #include "cartographer/mapping/internal/3d/imu_integration.h"
-#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h"
-#include "cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h"
-#include "cartographer/mapping/internal/3d/rotation_cost_function_3d.h"
 #include "cartographer/mapping/internal/3d/rotation_parameterization.h"
-#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h"
 #include "cartographer/transform/timestamped_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -46,7 +46,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 namespace {
 
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
@@ -188,7 +188,7 @@ void AddLandmarkCostFunctions(
 }  // namespace
 
 OptimizationProblem3D::OptimizationProblem3D(
-    const pose_graph::proto::OptimizationProblemOptions& options)
+    const optimization::proto::OptimizationProblemOptions& options)
     : options_(options) {}
 
 OptimizationProblem3D::~OptimizationProblem3D() {}
@@ -425,7 +425,7 @@ void OptimizationProblem3D::Solve(
               trajectory_data.imu_calibration.data());
         }
         problem.AddResidualBlock(
-            RotationCostFunction::CreateAutoDiffCostFunction(
+            RotationCostFunction3D::CreateAutoDiffCostFunction(
                 options_.rotation_weight(), result.delta_rotation),
             nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
             C_nodes.at(second_node_id).rotation(),
@@ -605,6 +605,6 @@ OptimizationProblem3D::CalculateOdometryBetweenNodes(
   return nullptr;
 }
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h b/cartographer/mapping/internal/optimization/optimization_problem_3d.h
similarity index 90%
rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h
rename to cartographer/mapping/internal/optimization/optimization_problem_3d.h
index 6c545fa..db1a475 100644
--- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h
+++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
 
 #include <array>
 #include <map>
@@ -28,7 +28,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/id.h"
-#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h"
 #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
@@ -39,7 +39,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 struct NodeSpec3D {
   common::Time time;
@@ -56,7 +56,7 @@ class OptimizationProblem3D
                                           transform::Rigid3d> {
  public:
   explicit OptimizationProblem3D(
-      const pose_graph::proto::OptimizationProblemOptions& options);
+      const optimization::proto::OptimizationProblemOptions& options);
   ~OptimizationProblem3D();
 
   OptimizationProblem3D(const OptimizationProblem3D&) = delete;
@@ -121,7 +121,7 @@ class OptimizationProblem3D
       int trajectory_id, const NodeSpec3D& first_node_data,
       const NodeSpec3D& second_node_data) const;
 
-  pose_graph::proto::OptimizationProblemOptions options_;
+  optimization::proto::OptimizationProblemOptions options_;
   MapById<NodeId, NodeSpec3D> node_data_;
   MapById<SubmapId, SubmapSpec3D> submap_data_;
   std::map<std::string, transform::Rigid3d> landmark_data_;
@@ -131,8 +131,8 @@ class OptimizationProblem3D
   std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
similarity index 95%
rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc
rename to cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
index 963fd55..facfb90 100644
--- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc
+++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
@@ -14,21 +14,21 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
 
 #include <random>
 
 #include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_options.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 #include "gmock/gmock.h"
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 namespace {
 
 class OptimizationProblem3DTest : public ::testing::Test {
@@ -36,7 +36,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
   OptimizationProblem3DTest()
       : optimization_problem_(CreateOptions()), rng_(45387) {}
 
-  pose_graph::proto::OptimizationProblemOptions CreateOptions() {
+  optimization::proto::OptimizationProblemOptions CreateOptions() {
     auto parameter_dictionary = common::MakeDictionary(R"text(
         return {
           acceleration_weight = 1e-4,
@@ -55,7 +55,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
             num_threads = 4,
           },
         })text");
-    return pose_graph::CreateOptimizationProblemOptions(
+    return optimization::CreateOptimizationProblemOptions(
         parameter_dictionary.get());
   }
 
@@ -191,6 +191,6 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
 }
 
 }  // namespace
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h b/cartographer/mapping/internal/optimization/optimization_problem_interface.h
similarity index 91%
rename from cartographer/mapping/internal/pose_graph/optimization_problem_interface.h
rename to cartographer/mapping/internal/optimization/optimization_problem_interface.h
index 24f41af..2725ec2 100644
--- a/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h
+++ b/cartographer/mapping/internal/optimization/optimization_problem_interface.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
 
 #include <map>
 #include <set>
@@ -33,7 +33,7 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 // Implements the SPA loop closure method.
 template <typename NodeDataType, typename SubmapDataType,
@@ -81,8 +81,8 @@ class OptimizationProblemInterface {
       const = 0;
 };
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc
similarity index 94%
rename from cartographer/mapping/internal/pose_graph/optimization_problem_options.cc
rename to cartographer/mapping/internal/optimization/optimization_problem_options.cc
index 7417eb0..360e4be 100644
--- a/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc
+++ b/cartographer/mapping/internal/optimization/optimization_problem_options.cc
@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h"
+#include "cartographer/mapping/internal/optimization/optimization_problem_options.h"
 
 #include "cartographer/common/ceres_solver_options.h"
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
@@ -50,6 +50,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
   return options;
 }
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_options.h b/cartographer/mapping/internal/optimization/optimization_problem_options.h
similarity index 76%
rename from cartographer/mapping/internal/pose_graph/optimization_problem_options.h
rename to cartographer/mapping/internal/optimization/optimization_problem_options.h
index d4b3a66..5182760 100644
--- a/cartographer/mapping/internal/pose_graph/optimization_problem_options.h
+++ b/cartographer/mapping/internal/optimization/optimization_problem_options.h
@@ -14,21 +14,21 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
+namespace optimization {
 
 proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
     common::LuaParameterDictionary* parameter_dictionary);
 
-}  // namespace pose_graph
+}  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index d601a9f..cc3359c 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -68,14 +68,14 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
   if (options.use_trajectory_builder_2d()) {
     pose_graph_ = common::make_unique<PoseGraph2D>(
         options_.pose_graph_options(),
-        common::make_unique<pose_graph::OptimizationProblem2D>(
+        common::make_unique<optimization::OptimizationProblem2D>(
             options_.pose_graph_options().optimization_problem_options()),
         &thread_pool_);
   }
   if (options.use_trajectory_builder_3d()) {
     pose_graph_ = common::make_unique<PoseGraph3D>(
         options_.pose_graph_options(),
-        common::make_unique<pose_graph::OptimizationProblem3D>(
+        common::make_unique<optimization::OptimizationProblem3D>(
             options_.pose_graph_options().optimization_problem_options()),
         &thread_pool_);
   }
diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc
index e5b843b..8d8601b 100644
--- a/cartographer/mapping/pose_graph.cc
+++ b/cartographer/mapping/pose_graph.cc
@@ -16,8 +16,8 @@
 
 #include "cartographer/mapping/pose_graph.h"
 
+#include "cartographer/mapping/internal/optimization/optimization_problem_options.h"
 #include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
-#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
@@ -82,7 +82,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
   options.set_matcher_rotation_weight(
       parameter_dictionary->GetDouble("matcher_rotation_weight"));
   *options.mutable_optimization_problem_options() =
-      pose_graph::CreateOptimizationProblemOptions(
+      optimization::CreateOptimizationProblemOptions(
           parameter_dictionary->GetDictionary("optimization_problem").get());
   options.set_max_num_final_iterations(
       parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));
diff --git a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto
index f399735..9994adb 100644
--- a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto
+++ b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto
@@ -14,7 +14,7 @@
 
 syntax = "proto3";
 
-package cartographer.mapping.pose_graph.proto;
+package cartographer.mapping.optimization.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
diff --git a/cartographer/mapping/proto/pose_graph_options.proto b/cartographer/mapping/proto/pose_graph_options.proto
index 31cdfc8..5de3a2a 100644
--- a/cartographer/mapping/proto/pose_graph_options.proto
+++ b/cartographer/mapping/proto/pose_graph_options.proto
@@ -37,7 +37,7 @@ message PoseGraphOptions {
   double matcher_rotation_weight = 8;
 
   // Options for the optimization problem.
-  mapping.pose_graph.proto.OptimizationProblemOptions
+  mapping.optimization.proto.OptimizationProblemOptions
       optimization_problem_options = 4;
 
   // Number of iterations to use in 'optimization_problem_options' for the final