diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc new file mode 100644 index 0000000..5e30035 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc @@ -0,0 +1,40 @@ +/* + * 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_3d.h" + +namespace cartographer { +namespace pose_graph { + +InterpolatedRelativePoseCost3D::InterpolatedRelativePoseCost3D( + const proto::InterpolatedRelativePose3D::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())) {} + +proto::InterpolatedRelativePose3D::Parameters +InterpolatedRelativePoseCost3D::ToProto() const { + proto::InterpolatedRelativePose3D::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_); + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer \ No newline at end of file diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h new file mode 100644 index 0000000..dab6c9c --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h @@ -0,0 +1,71 @@ +/* + * 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_3D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_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 interpolated relative pose in 3d and +// de/serialization methods; only the first pose is linearly interpolated +// between two nodes. +class InterpolatedRelativePoseCost3D { + public: + InterpolatedRelativePoseCost3D( + const proto::InterpolatedRelativePose3D::Parameters& parameters); + + proto::InterpolatedRelativePose3D::Parameters ToProto() const; + + template + bool operator()(const T* const first_start_translation, + const T* const first_start_rotation, + const T* const first_end_translation, + const T* const first_end_rotation, + const T* const second_translation, + const T* const second_rotation, T* const error_out) const { + const std::tuple, std::array> + interpolated_rotation_and_translation = + mapping::optimization::InterpolateNodes3D( + first_start_rotation, first_start_translation, + first_end_rotation, first_end_translation, + interpolation_factor_); + const std::array error = mapping::optimization::ScaleError( + mapping::optimization::ComputeUnscaledError( + first_T_second_, + std::get<0>(interpolated_rotation_and_translation).data(), + std::get<1>(interpolated_rotation_and_translation).data(), + second_rotation, second_translation), + translation_weight_, rotation_weight_); + std::copy(std::begin(error), std::end(error), error_out); + return true; + } + + private: + const double translation_weight_; + const double rotation_weight_; + const double interpolation_factor_; + const transform::Rigid3d first_T_second_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc new file mode 100644 index 0000000..f5e88a0 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_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_3d.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 PositionType = 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: 10 + interpolation_factor: 0.3 +)PROTO"; + +TEST(InterpolatedRelativePoseCost3DTest, SerializesCorrectly) { + const auto parameters = + ParseProto(kParameters); + InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters); + const auto actual_proto = interpolated_relative_pose_cost_3d.ToProto(); + EXPECT_THAT(actual_proto, EqualsProto(kParameters)); +} + +TEST(InterpolatedRelativePoseCost3DTest, EvaluatesCorrectly) { + const auto parameters = + ParseProto(kParameters); + InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters); + + const PositionType kFirstStartPosition{{1., 1., 1.}}; + const RotationType kFirstStartRotation{{1., 1., 1., 1.}}; + const PositionType kFirstEndPosition{{2., 3., 4.}}; + const RotationType kFirstEndRotation{{1.1, 1.2, 1.3, 1.4}}; + const PositionType kSecondPosition{{0., -1., -2.}}; + const RotationType kSecondRotation{{.1, .2, .3, .4}}; + + ResidualType residuals; + EXPECT_TRUE(interpolated_relative_pose_cost_3d( + kFirstStartPosition.data(), kFirstStartRotation.data(), + kFirstEndPosition.data(), kFirstEndRotation.data(), + kSecondPosition.data(), kSecondRotation.data(), residuals.data())); + EXPECT_THAT(residuals, + ElementsAre(Near(8.4594), Near(10.27735), Near(-4.45472), + Near(0.968852), Near(11.96531), Near(3.34254))); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc new file mode 100644 index 0000000..d81d84e --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc @@ -0,0 +1,102 @@ +/* + * 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_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 + +InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D( + const ConstraintId& id, const proto::LossFunction& loss_function_proto, + const proto::InterpolatedRelativePose3D& proto) + : Constraint(id, loss_function_proto), + first_start_(proto.first_start()), + first_end_(proto.first_end()), + second_(proto.second()), + cost_(new InterpolatedRelativePoseCost3D(proto.parameters())), + ceres_cost_(common::make_unique(cost_)) {} + +void InterpolatedRelativePoseConstraint3D::AddToOptimizer( + Nodes* nodes, ceres::Problem* problem) const { + auto first_node_start = + common::FindOrNull(nodes->pose_3d_nodes, first_start_); + if (first_node_start == nullptr) { + LOG(INFO) << "First node (start) was not found in pose_3d_nodes."; + return; + } + + auto first_node_end = common::FindOrNull(nodes->pose_3d_nodes, first_end_); + if (first_node_end == nullptr) { + LOG(INFO) << "First node (end) 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_start->constant() && first_node_end->constant() && + second_node->constant()) { + LOG(INFO) << "All nodes are constant, skipping the constraint."; + return; + } + + AddPoseParameters(first_node_start, problem); + AddPoseParameters(first_node_end, problem); + AddPoseParameters(second_node, problem); + problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */, + first_node_start->mutable_translation()->data(), + first_node_start->mutable_rotation()->data(), + first_node_end->mutable_translation()->data(), + first_node_end->mutable_rotation()->data(), + second_node->mutable_translation()->data(), + second_node->mutable_rotation()->data()); +} + +proto::CostFunction InterpolatedRelativePoseConstraint3D::ToCostFunctionProto() + const { + proto::CostFunction cost_function; + auto* interpolated_relative_pose_3d = + cost_function.mutable_interpolated_relative_pose_3d(); + *interpolated_relative_pose_3d->mutable_first_start() = + first_start_.ToProto(); + *interpolated_relative_pose_3d->mutable_first_end() = first_end_.ToProto(); + *interpolated_relative_pose_3d->mutable_second() = second_.ToProto(); + *interpolated_relative_pose_3d->mutable_parameters() = cost_->ToProto(); + return cost_function; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h new file mode 100644 index 0000000..86c35b9 --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h @@ -0,0 +1,61 @@ +/* + * 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_3D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h" + +namespace cartographer { +namespace pose_graph { + +class InterpolatedRelativePoseConstraint3D : public Constraint { + public: + InterpolatedRelativePoseConstraint3D( + const ConstraintId& id, const proto::LossFunction& loss_function_proto, + const proto::InterpolatedRelativePose3D& proto); + + void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + + protected: + proto::CostFunction ToCostFunctionProto() const final; + + private: + // clang-format off + using AutoDiffFunction = ceres::AutoDiffCostFunction< + InterpolatedRelativePoseCost3D, + 6 /* number of residuals */, + 3 /* translation of first start pose */, + 4 /* rotation of first start pose */, + 3 /* translation of first end pose */, + 4 /* rotation 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. + InterpolatedRelativePoseCost3D* const cost_; + std::unique_ptr ceres_cost_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc new file mode 100644 index 0000000..642188a --- /dev/null +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc @@ -0,0 +1,67 @@ +/* + * 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_3d.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 PositionType = std::array; +using RotationType = std::array; +using ResidualType = std::array; + +constexpr char kConstraint[] = R"PROTO( + id: "narf" + cost_function { + interpolated_relative_pose_3d { + 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 + interpolation_factor: 0.3 + } + } + } + + loss_function { quadratic_loss {} } +)PROTO"; + +TEST(InterpolatedRelativePostConstraint3DTest, SerializesCorrectly) { + const auto proto = ParseProto(kConstraint); + InterpolatedRelativePoseConstraint3D constraint( + proto.id(), proto.loss_function(), + proto.cost_function().interpolated_relative_pose_3d()); + 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 bdd2f87..63d85f3 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -88,19 +88,19 @@ message RelativePose2DInterpolated { Parameters parameters = 3; } -message RelativePose3DInterpolated { - InterpolatedNode first = 1; - NodeId second = 2; +message InterpolatedRelativePose3D { + 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_start = 4; - transform.proto.Quaterniond gravity_alignment_end = 5; + // Interpolates between first_start and first_end. + double interpolation_factor = 4; } - Parameters parameters = 3; + Parameters parameters = 4; } message CostFunction { @@ -110,6 +110,6 @@ message CostFunction { Acceleration3D acceleration_3d = 3; Rotation3D rotation_3d = 4; RelativePose2DInterpolated relative_pose_2d_interpolated = 5; - RelativePose3DInterpolated relative_pose_3d_interpolated = 6; + InterpolatedRelativePose3D interpolated_relative_pose_3d = 6; } }