From d5840e960a79c11fe8c04675b17c5f7844853d3b Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 7 Aug 2018 14:48:39 +0200 Subject: [PATCH] [GenericPoseGraph] Move functions that add param blocks to constraint_utils. (#1374) Also call `ceres_loss()` instead of passing `nullptr`. --- .../constraint/acceleration_constraint_3d.cc | 34 +++-------------- .../constraint/acceleration_constraint_3d.h | 2 +- .../pose_graph/constraint/constraint_utils.cc | 37 +++++++++++++++++++ .../pose_graph/constraint/constraint_utils.h | 37 +++++++++++++++++++ ...nterpolated_relative_pose_constraint_2d.cc | 29 ++------------- ...nterpolated_relative_pose_constraint_3d.cc | 23 +++--------- .../constraint/relative_pose_constraint_2d.cc | 1 + .../constraint/relative_pose_constraint_3d.cc | 21 ++--------- .../constraint/rotation_constraint_3d.cc | 29 ++++++++------- 9 files changed, 110 insertions(+), 103 deletions(-) create mode 100644 cartographer/pose_graph/constraint/constraint_utils.cc create mode 100644 cartographer/pose_graph/constraint/constraint_utils.h diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc index 5972197..b7fbdbd 100644 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc +++ b/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(), diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h b/cartographer/pose_graph/constraint/acceleration_constraint_3d.h index b39d323..97c5b82 100644 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h +++ b/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_ diff --git a/cartographer/pose_graph/constraint/constraint_utils.cc b/cartographer/pose_graph/constraint/constraint_utils.cc new file mode 100644 index 0000000..216f81b --- /dev/null +++ b/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 diff --git a/cartographer/pose_graph/constraint/constraint_utils.h b/cartographer/pose_graph/constraint/constraint_utils.h new file mode 100644 index 0000000..f667fcf --- /dev/null +++ b/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_ diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc index 39287f2..e164b82 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc +++ b/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(), diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc index 3c3cbf1..059bc65 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc +++ b/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(), diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc index 39da926..7fa34e9 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc +++ b/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 { diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc index ee5f177..fe4a717 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc +++ b/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(), diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc index 92d4338..2455f2d 100644 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc +++ b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc @@ -23,7 +23,7 @@ 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()) { @@ -31,6 +31,14 @@ void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) { } } +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 {