Allow easier access to CeresPose's data_ (#1149)

Introduce a FromPose function which transforms a Ridig3d pose into the
CeresPose's Data format.

cartographer/mapping/internal/optimization/ceres_pose.cc

@@ -20,16 +20,19 @@ namespace cartographer {
 namespace mapping {
 namespace optimization {
 
+CeresPose::Data FromPose(const transform::Rigid3d& pose) {
+  return CeresPose::Data{{{pose.translation().x(), pose.translation().y(),
+                           pose.translation().z()}},
+                         {{pose.rotation().w(), pose.rotation().x(),
+                           pose.rotation().y(), pose.rotation().z()}}};
+}
+
 CeresPose::CeresPose(
-    const transform::Rigid3d& rigid,
+    const transform::Rigid3d& pose,
     std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
     std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
     ceres::Problem* problem)
-    : data_(std::make_shared<CeresPose::Data>(
-          CeresPose::Data{{{rigid.translation().x(), rigid.translation().y(),
-                            rigid.translation().z()}},
-                          {{rigid.rotation().w(), rigid.rotation().x(),
-                            rigid.rotation().y(), rigid.rotation().z()}}})) {
+    : data_(std::make_shared<CeresPose::Data>(FromPose(pose))) {
   problem->AddParameterBlock(data_->translation.data(), 3,
                              translation_parametrization.release());
   problem->AddParameterBlock(data_->rotation.data(), 4,

cartographer/mapping/internal/optimization/ceres_pose.h

@@ -44,15 +44,20 @@ class CeresPose {
   double* rotation() { return data_->rotation.data(); }
   const double* rotation() const { return data_->rotation.data(); }
 
- private:
   struct Data {
     std::array<double, 3> translation;
     // Rotation quaternion as (w, x, y, z).
     std::array<double, 4> rotation;
   };
+
+  Data& data() { return *data_; }
+
+ private:
   std::shared_ptr<Data> data_;
 };
 
+CeresPose::Data FromPose(const transform::Rigid3d& pose);
+
 }  // namespace optimization
 }  // namespace mapping
 }  // namespace cartographer