From c2c341397f9a69195e1fc3aabac240845737b1c6 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 26 Jul 2018 15:00:13 +0200 Subject: [PATCH] [GenericPoseGraph] Add InterpolatedRelativePose2D constraint. (#1337) --- .../interpolated_relative_pose_cost_2d.cc | 48 ++++++++ .../interpolated_relative_pose_cost_2d.h | 66 +++++++++++ ...interpolated_relative_pose_cost_2d_test.cc | 76 ++++++++++++ ...nterpolated_relative_pose_constraint_2d.cc | 108 ++++++++++++++++++ ...interpolated_relative_pose_constraint_2d.h | 59 ++++++++++ ...olated_relative_pose_constraint_2d_test.cc | 62 ++++++++++ .../pose_graph/proto/cost_function.proto | 21 ++-- 7 files changed, 429 insertions(+), 11 deletions(-) create mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc create mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h create mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc create mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc create mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h create mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc new file mode 100644 index 0000000..b09c74e --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc @@ -0,0 +1,48 @@ +/* + * 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/cost_function/interpolated_relative_pose_cost_2d.h" + +namespace cartographer { +namespace pose_graph { + +InterpolatedRelativePoseCost2D::InterpolatedRelativePoseCost2D( + const proto::InterpolatedRelativePose2D::Parameters& parameters) + : translation_weight_(parameters.translation_weight()), + rotation_weight_(parameters.rotation_weight()), + interpolation_factor_(parameters.interpolation_factor()), + first_T_second_(transform::ToRigid3(parameters.first_t_second())), + gravity_alignment_first_start_( + transform::ToEigen(parameters.gravity_alignment_first_start())), + gravity_alignment_first_end_( + transform::ToEigen(parameters.gravity_alignment_first_end())) {} + +proto::InterpolatedRelativePose2D::Parameters +InterpolatedRelativePoseCost2D::ToProto() const { + proto::InterpolatedRelativePose2D::Parameters parameters; + parameters.set_translation_weight(translation_weight_); + parameters.set_rotation_weight(rotation_weight_); + parameters.set_interpolation_factor(interpolation_factor_); + *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_); + *parameters.mutable_gravity_alignment_first_start() = + transform::ToProto(gravity_alignment_first_start_); + *parameters.mutable_gravity_alignment_first_end() = + transform::ToProto(gravity_alignment_first_end_); + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h new file mode 100644 index 0000000..e86dfb5 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h @@ -0,0 +1,66 @@ +/* + * 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_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H + +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/pose_graph/proto/cost_function.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace pose_graph { + +// Provides cost function for relative pose and de/serialization methods. +class InterpolatedRelativePoseCost2D { + public: + explicit InterpolatedRelativePoseCost2D( + const proto::InterpolatedRelativePose2D::Parameters& parameters); + + proto::InterpolatedRelativePose2D::Parameters ToProto() const; + + template + bool operator()(const T* const first_start_pose, + const T* const first_end_pose, + const T* const second_translation, + const T* const second_rotation, T* const e) const { + const std::tuple, std::array> + interpolated_first_pose = mapping::optimization::InterpolateNodes2D( + first_start_pose, gravity_alignment_first_start_, first_end_pose, + gravity_alignment_first_end_, interpolation_factor_); + const std::array error = mapping::optimization::ScaleError( + mapping::optimization::ComputeUnscaledError( + first_T_second_, std::get<0>(interpolated_first_pose).data(), + std::get<1>(interpolated_first_pose).data(), second_rotation, + second_translation), + translation_weight_, rotation_weight_); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + const double translation_weight_; + const double rotation_weight_; + const double interpolation_factor_; + const transform::Rigid3d first_T_second_; + const Eigen::Quaterniond gravity_alignment_first_start_; + const Eigen::Quaterniond gravity_alignment_first_end_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc new file mode 100644 index 0000000..7464e10 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc @@ -0,0 +1,76 @@ +/* + * 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/cost_function/interpolated_relative_pose_cost_2d.h" + +#include "cartographer/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using ::testing::ElementsAre; +using testing::EqualsProto; +using testing::Near; +using testing::ParseProto; + +using Position2DType = std::array; +using TranslationType = std::array; +using RotationType = std::array; +using ResidualType = std::array; + +constexpr char kParameters[] = R"PROTO( + first_t_second { + translation: { x: 1 y: 2 z: 3 } + rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } + } + translation_weight: 1 + rotation_weight: 2 + gravity_alignment_first_start { x: 0.4 y: 0.1 z: 0.3 w: 0.2 } + gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 } + interpolation_factor: 0.3 +)PROTO"; + +TEST(InterpolatedRelativePoseCost2DTest, SerializesCorrectly) { + const auto parameters = + ParseProto(kParameters); + InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters); + const auto actual_proto = interpolated_relative_pose_cost_2d.ToProto(); + EXPECT_THAT(actual_proto, EqualsProto(kParameters)); +} + +TEST(InterpolatedRelativePoseCost2DTest, EvaluatesCorrectly) { + const auto parameters = + ParseProto(kParameters); + InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters); + + const Position2DType kFirstStartPose{{1., 1., 1.}}; + const Position2DType kFirstEndPose{{2., 3., 4.}}; + const TranslationType kSecondTranslation{{0., -1., -2.}}; + const RotationType kSecondRotation{{.4, .2, .3, .1}}; + + ResidualType residuals; + EXPECT_TRUE(interpolated_relative_pose_cost_2d( + kFirstStartPose.data(), kFirstEndPose.data(), kSecondTranslation.data(), + kSecondRotation.data(), residuals.data())); + EXPECT_THAT(residuals, + ElementsAre(Near(4.49044), Near(1.8527), Near(3.49511), + Near(-1.93746), Near(3.54854), Near(4.50243))); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc new file mode 100644 index 0000000..26a43c8 --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc @@ -0,0 +1,108 @@ +/* + * 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/interpolated_relative_pose_constraint_2d.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/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, + const proto::InterpolatedRelativePose2D& proto) + : Constraint(id, loss_function_proto), + first_start_(proto.first_start()), + first_end_(proto.first_end()), + second_(proto.second()), + cost_(new InterpolatedRelativePoseCost2D(proto.parameters())), + ceres_cost_(common::make_unique(cost_)) {} + +void InterpolatedRelativePoseConstraint2D::AddToOptimizer( + Nodes* nodes, ceres::Problem* problem) const { + auto first_node_start = + common::FindOrNull(nodes->pose_2d_nodes, first_start_); + if (first_node_start == nullptr) { + LOG(INFO) << "First node (start) was not found in pose_2d_nodes."; + return; + } + + auto first_node_end = common::FindOrNull(nodes->pose_2d_nodes, first_end_); + if (first_node_end == nullptr) { + LOG(INFO) << "First node (end) was not found in pose_2d_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_start->constant() && first_node_end->constant() && + second_node->constant()) { + LOG(INFO) << "All nodes are constant, skipping the constraint."; + return; + } + + AddPose2DParameters(first_node_start, problem); + AddPose2DParameters(first_node_end, problem); + AddPose3DParameters(second_node, problem); + problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), + first_node_start->mutable_pose_2d()->data(), + first_node_end->mutable_pose_2d()->data(), + second_node->mutable_translation()->data(), + second_node->mutable_rotation()->data()); +} + +proto::CostFunction InterpolatedRelativePoseConstraint2D::ToCostFunctionProto() + const { + proto::CostFunction cost_function; + auto* interpolated_relative_pose_2d = + cost_function.mutable_interpolated_relative_pose_2d(); + *interpolated_relative_pose_2d->mutable_first_start() = + first_start_.ToProto(); + *interpolated_relative_pose_2d->mutable_first_end() = first_end_.ToProto(); + *interpolated_relative_pose_2d->mutable_second() = second_.ToProto(); + *interpolated_relative_pose_2d->mutable_parameters() = cost_->ToProto(); + return cost_function; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h new file mode 100644 index 0000000..1260733 --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h @@ -0,0 +1,59 @@ +/* + * 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_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h" + +namespace cartographer { +namespace pose_graph { + +class InterpolatedRelativePoseConstraint2D : public Constraint { + public: + InterpolatedRelativePoseConstraint2D( + const ConstraintId& id, const proto::LossFunction& loss_function_proto, + const proto::InterpolatedRelativePose2D& proto); + + void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + + protected: + proto::CostFunction ToCostFunctionProto() const final; + + private: + // clang-format off + using AutoDiffFunction = ceres::AutoDiffCostFunction< + InterpolatedRelativePoseCost2D, + 6 /* residuals */, + 3 /* 2d pose variables of first start pose */, + 3 /* 2d pose variables of first end pose */, + 3 /* translation of second pose */, + 4 /* rotation of second pose */>; + // clang-format on + NodeId first_start_; + NodeId first_end_; + NodeId second_; + + // The cost function is owned by the ceres cost function. + InterpolatedRelativePoseCost2D* const cost_; + std::unique_ptr ceres_cost_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc new file mode 100644 index 0000000..8659876 --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc @@ -0,0 +1,62 @@ +/* + * 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/interpolated_relative_pose_constraint_2d.h" + +#include "cartographer/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using testing::EqualsProto; +using testing::ParseProto; + +constexpr char kConstraint[] = R"PROTO( + id: "narf" + cost_function { + interpolated_relative_pose_2d { + first_start { object_id: "node0_start" } + first_end { object_id: "node0_end" } + second { object_id: "node1" } + parameters { + first_t_second { + translation: { x: 1 y: 2 z: 3 } + rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } + } + translation_weight: 0.5 + rotation_weight: 1.0 + gravity_alignment_first_start { x: 0.4 y: 0.2 z: 0.3 w: 0.2 } + gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 } + interpolation_factor: 0.3 + } + } + } + loss_function { quadratic_loss {} } +)PROTO"; + +TEST(InterpolatedRelativePostConstraint2DTest, SerializesCorrectly) { + const auto proto = ParseProto(kConstraint); + InterpolatedRelativePoseConstraint2D constraint( + proto.id(), proto.loss_function(), + proto.cost_function().interpolated_relative_pose_2d()); + const auto actual_proto = constraint.ToProto(); + EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/proto/cost_function.proto b/cartographer/pose_graph/proto/cost_function.proto index 63d85f3..61f8298 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -70,22 +70,21 @@ message Rotation3D { Parameters parameters = 4; } -message InterpolatedNode { - NodeId start = 1; - NodeId end = 2; - double factor = 3; -} - -message RelativePose2DInterpolated { - InterpolatedNode first = 1; - NodeId second = 2; +message InterpolatedRelativePose2D { + NodeId first_start = 1; + NodeId first_end = 2; + NodeId second = 3; message Parameters { transform.proto.Rigid3d first_t_second = 1; double translation_weight = 2; double rotation_weight = 3; + transform.proto.Quaterniond gravity_alignment_first_start = 4; + transform.proto.Quaterniond gravity_alignment_first_end = 5; + // Interpolates between first_start and first_end. + double interpolation_factor = 6; } - Parameters parameters = 3; + Parameters parameters = 4; } message InterpolatedRelativePose3D { @@ -109,7 +108,7 @@ message CostFunction { RelativePose3D relative_pose_3d = 2; Acceleration3D acceleration_3d = 3; Rotation3D rotation_3d = 4; - RelativePose2DInterpolated relative_pose_2d_interpolated = 5; + InterpolatedRelativePose2D interpolated_relative_pose_2d = 5; InterpolatedRelativePose3D interpolated_relative_pose_3d = 6; } }