From bb80d78293407176cb0294c807ac4c93269039ab Mon Sep 17 00:00:00 2001 From: Martin Bokeloh Date: Fri, 20 Jul 2018 16:34:27 +0200 Subject: [PATCH] [GenericPoseGraph] Add relative pose constraint 3d. (#1309) --- .../constraint/relative_pose_constraint_3d.cc | 86 +++++++++++++++++++ .../constraint/relative_pose_constraint_3d.h | 55 ++++++++++++ 2 files changed, 141 insertions(+) create mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc create mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_3d.h diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc new file mode 100644 index 0000000..4bd7527 --- /dev/null +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc @@ -0,0 +1,86 @@ +/* + * 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. + */ + +#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/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, + const proto::RelativePose3D& proto) + : Constraint(id, loss_function_proto), + first_(proto.first()), + second_(proto.second()), + cost_(new RelativePoseCost3D(proto.parameters())), + ceres_cost_(common::make_unique(cost_)) {} + +void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes, + ceres::Problem* problem) const { + auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_); + if (first_node == nullptr) { + LOG(INFO) << "First node was not found in pose_3d_nodes."; + return; + } + + auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_); + if (second_node == nullptr) { + LOG(INFO) << "Second node was not found in pose_3d_nodes."; + return; + } + + if (first_node->constant() && second_node->constant()) { + LOG(INFO) << "Both nodes are constant, skipping the constraint."; + return; + } + + AddPoseParameters(first_node, problem); + AddPoseParameters(second_node, problem); + problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */, + first_node->mutable_translation()->data(), + first_node->mutable_rotation()->data(), + second_node->mutable_translation()->data(), + second_node->mutable_rotation()->data()); +} + +proto::CostFunction RelativePoseConstraint3D::ToCostFunctionProto() const { + proto::CostFunction cost_function; + auto* relative_pose_3d = cost_function.mutable_relative_pose_3d(); + *relative_pose_3d->mutable_first() = first_.ToProto(); + *relative_pose_3d->mutable_second() = second_.ToProto(); + *relative_pose_3d->mutable_parameters() = cost_->ToProto(); + return cost_function; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h new file mode 100644 index 0000000..f6950cd --- /dev/null +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h @@ -0,0 +1,55 @@ +/* + * 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_RELATIVE_POSE_CONSTRAINT_3D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" + +namespace cartographer { +namespace pose_graph { + +class RelativePoseConstraint3D : public Constraint { + public: + RelativePoseConstraint3D(const ConstraintId& id, + const proto::LossFunction& loss_function_proto, + const proto::RelativePose3D& proto); + + void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + + protected: + proto::CostFunction ToCostFunctionProto() const final; + + private: + using AutoDiffFunction = + ceres::AutoDiffCostFunction; + NodeId first_; + NodeId second_; + // The cost function is owned by the ceres cost function. + RelativePoseCost3D* const cost_; + std::unique_ptr ceres_cost_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_