[GenericPoseGraph] Move functions that add param blocks to constraint…

…_utils. (#1374)

Also call `ceres_loss()` instead of passing `nullptr`.

cartographer/pose_graph/constraint/acceleration_constraint_3d.cc

@@ -18,34 +18,10 @@
 
 #include "absl/memory/memory.h"
 #include "cartographer/common/utils.h"
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
 
 namespace cartographer {
 namespace pose_graph {
-namespace {
-
-void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
-  auto transation = pose->mutable_translation();
-  auto rotation = pose->mutable_rotation();
-  problem->AddParameterBlock(transation->data(), transation->size());
-  problem->AddParameterBlock(rotation->data(), rotation->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(transation->data());
-    problem->SetParameterBlockConstant(rotation->data());
-  }
-}
-
-void AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) {
-  auto gravity = pose->mutable_gravity_constant();
-  auto orientation = pose->mutable_orientation();
-  problem->AddParameterBlock(gravity, 1);
-  problem->AddParameterBlock(orientation->data(), orientation->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(gravity);
-    problem->SetParameterBlockConstant(orientation->data());
-  }
-}
-
-}  // namespace
 
 AccelerationConstraint3D::AccelerationConstraint3D(
     const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -90,10 +66,10 @@ void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
     return;
   }
 
-  AddPoseParameters(first_node, problem);
-  AddPoseParameters(second_node, problem);
-  AddPoseParameters(third_node, problem);
-  AddImuParameters(imu_node, problem);
+  AddPose3D(first_node, problem);
+  AddPose3D(second_node, problem);
+  AddPose3D(third_node, problem);
+  AddImuCalibration(imu_node, problem);
   problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
                             second_node->mutable_rotation()->data(),
                             second_node->mutable_translation()->data(),

cartographer/pose_graph/constraint/acceleration_constraint_3d.h

@@ -58,4 +58,4 @@ class AccelerationConstraint3D : public Constraint {
 }  // namespace pose_graph
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_

cartographer/pose_graph/constraint/constraint_utils.cc

@@ -0,0 +1,37 @@
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+void AddPose2D(Pose2D* pose, ceres::Problem* problem) {
+  auto pose_2d = pose->mutable_pose_2d();
+  problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(pose_2d->data());
+  }
+}
+
+void AddPose3D(Pose3D* pose, ceres::Problem* problem) {
+  auto transation = pose->mutable_translation();
+  auto rotation = pose->mutable_rotation();
+  problem->AddParameterBlock(transation->data(), transation->size());
+  problem->AddParameterBlock(rotation->data(), rotation->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(transation->data());
+    problem->SetParameterBlockConstant(rotation->data());
+  }
+}
+
+void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) {
+  auto gravity = pose->mutable_gravity_constant();
+  auto orientation = pose->mutable_orientation();
+  problem->AddParameterBlock(gravity, 1);
+  problem->AddParameterBlock(orientation->data(), orientation->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(gravity);
+    problem->SetParameterBlockConstant(orientation->data());
+  }
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/constraint_utils.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
+#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
+
+#include "cartographer/pose_graph/node/imu_calibration.h"
+#include "cartographer/pose_graph/node/pose_2d.h"
+#include "cartographer/pose_graph/node/pose_3d.h"
+#include "ceres/problem.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+void AddPose2D(Pose2D* pose, ceres::Problem* problem);
+
+void AddPose3D(Pose3D* pose, ceres::Problem* problem);
+
+void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_

cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc

@@ -18,31 +18,10 @@
 
 #include "absl/memory/memory.h"
 #include "cartographer/common/utils.h"
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
 
 namespace cartographer {
 namespace pose_graph {
-namespace {
-
-void AddPose3DParameters(Pose3D* pose, ceres::Problem* problem) {
-  auto transation = pose->mutable_translation();
-  auto rotation = pose->mutable_rotation();
-  problem->AddParameterBlock(transation->data(), transation->size());
-  problem->AddParameterBlock(rotation->data(), rotation->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(transation->data());
-    problem->SetParameterBlockConstant(rotation->data());
-  }
-}
-
-void AddPose2DParameters(Pose2D* pose, ceres::Problem* problem) {
-  auto pose_2d = pose->mutable_pose_2d();
-  problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(pose_2d->data());
-  }
-}
-
-}  // namespace
 
 InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
     const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -81,9 +60,9 @@ void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
     return;
   }
 
-  AddPose2DParameters(first_node_start, problem);
-  AddPose2DParameters(first_node_end, problem);
-  AddPose3DParameters(second_node, problem);
+  AddPose2D(first_node_start, problem);
+  AddPose2D(first_node_end, problem);
+  AddPose3D(second_node, problem);
   problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
                             first_node_start->mutable_pose_2d()->data(),
                             first_node_end->mutable_pose_2d()->data(),

cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc

@@ -18,23 +18,10 @@
 
 #include "absl/memory/memory.h"
 #include "cartographer/common/utils.h"
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
 
 namespace cartographer {
 namespace pose_graph {
-namespace {
-
-void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
-  auto transation = pose->mutable_translation();
-  auto rotation = pose->mutable_rotation();
-  problem->AddParameterBlock(transation->data(), transation->size());
-  problem->AddParameterBlock(rotation->data(), rotation->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(transation->data());
-    problem->SetParameterBlockConstant(rotation->data());
-  }
-}
-
-}  // namespace
 
 InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
     const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -73,10 +60,10 @@ void InterpolatedRelativePoseConstraint3D::AddToOptimizer(
     return;
   }
 
-  AddPoseParameters(first_node_start, problem);
-  AddPoseParameters(first_node_end, problem);
-  AddPoseParameters(second_node, problem);
-  problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
+  AddPose3D(first_node_start, problem);
+  AddPose3D(first_node_end, problem);
+  AddPose3D(second_node, problem);
+  problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
                             first_node_start->mutable_translation()->data(),
                             first_node_start->mutable_rotation()->data(),
                             first_node_end->mutable_translation()->data(),

cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc

@@ -18,6 +18,7 @@
 
 #include "absl/memory/memory.h"
 #include "cartographer/common/utils.h"
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
 
 namespace cartographer {
 namespace pose_graph {

cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc

@@ -18,23 +18,10 @@
 
 #include "absl/memory/memory.h"
 #include "cartographer/common/utils.h"
+#include "cartographer/pose_graph/constraint/constraint_utils.h"
 
 namespace cartographer {
 namespace pose_graph {
-namespace {
-
-void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
-  auto transation = pose->mutable_translation();
-  auto rotation = pose->mutable_rotation();
-  problem->AddParameterBlock(transation->data(), transation->size());
-  problem->AddParameterBlock(rotation->data(), rotation->size());
-  if (pose->constant()) {
-    problem->SetParameterBlockConstant(transation->data());
-    problem->SetParameterBlockConstant(rotation->data());
-  }
-}
-
-}  // namespace
 
 RelativePoseConstraint3D::RelativePoseConstraint3D(
     const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -64,9 +51,9 @@ void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes,
     return;
   }
 
-  AddPoseParameters(first_node, problem);
-  AddPoseParameters(second_node, problem);
-  problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
+  AddPose3D(first_node, problem);
+  AddPose3D(second_node, problem);
+  problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
                             first_node->mutable_translation()->data(),
                             first_node->mutable_rotation()->data(),
                             second_node->mutable_translation()->data(),

cartographer/pose_graph/constraint/rotation_constraint_3d.cc

@@ -23,14 +23,22 @@ namespace cartographer {
 namespace pose_graph {
 namespace {
 
-void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) {
+void AddRotation3D(Pose3D* pose, ceres::Problem* problem) {
   auto rotation = pose->mutable_rotation();
   problem->AddParameterBlock(rotation->data(), rotation->size());
   if (pose->constant()) {
     problem->SetParameterBlockConstant(rotation->data());
   }
 }
 
+void AddImuOrientation(ImuCalibration* imu_node, ceres::Problem* problem) {
+  auto imu_orientation = imu_node->mutable_orientation();
+  problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
+  if (imu_node->constant()) {
+    problem->SetParameterBlockConstant(imu_orientation->data());
+  }
+}
+
 }  // namespace
 
 RotationContraint3D::RotationContraint3D(
@@ -62,25 +70,20 @@ void RotationContraint3D::AddToOptimizer(Nodes* nodes,
     return;
   }
 
-  auto imu_calibration_node =
+  auto imu_node =
       common::FindOrNull(nodes->imu_calibration_nodes, imu_calibration_);
-  if (imu_calibration_node == nullptr) {
+  if (imu_node == nullptr) {
     LOG(INFO) << "Imu calibration node was not found.";
     return;
   }
 
-  AddRotationParameters(first_node, problem);
-  AddRotationParameters(second_node, problem);
-  auto imu_orientation = imu_calibration_node->mutable_orientation();
-  problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
-  if (imu_calibration_node->constant()) {
-    problem->SetParameterBlockConstant(imu_orientation->data());
-  }
-
-  problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
+  AddRotation3D(first_node, problem);
+  AddRotation3D(second_node, problem);
+  AddImuOrientation(imu_node, problem);
+  problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
                             first_node->mutable_rotation()->data(),
                             second_node->mutable_rotation()->data(),
-                            imu_orientation->data());
+                            imu_node->mutable_orientation()->data());
 }
 
 proto::CostFunction RotationContraint3D::ToCostFunctionProto() const {