diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc new file mode 100644 index 0000000..9293fd9 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc @@ -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. + */ + +#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" + +namespace cartographer { +namespace pose_graph { + +RelativePoseCost3D::RelativePoseCost3D( + const proto::RelativePose3D::Parameters& parameters) + : translation_weight_(parameters.translation_weight()), + rotation_weight_(parameters.rotation_weight()), + first_T_second_(transform::ToRigid3(parameters.first_t_second())) {} + +proto::RelativePose3D::Parameters RelativePoseCost3D::ToProto() const { + proto::RelativePose3D::Parameters parameters; + parameters.set_translation_weight(translation_weight_); + parameters.set_rotation_weight(rotation_weight_); + *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/relative_pose_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h new file mode 100644 index 0000000..9c68a7e --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h @@ -0,0 +1,57 @@ +/* + * 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_RELATIVE_POSE_COST_3D_H +#define CARTOGRAPHER_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 relative pose and de/serialization methods. +class RelativePoseCost3D { + public: + explicit RelativePoseCost3D( + const proto::RelativePose3D::Parameters& parameters); + + proto::RelativePose3D::Parameters ToProto() const; + + template + bool operator()(const T* const c_i_translation, const T* const c_i_rotation, + const T* const c_j_translation, const T* const c_j_rotation, + T* const error_out) const { + const std::array error = mapping::optimization::ScaleError( + mapping::optimization::ComputeUnscaledError( + first_T_second_, c_i_rotation, c_i_translation, c_j_rotation, + c_j_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 transform::Rigid3d first_T_second_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_RELATIVE_POSE_COST_3D_H diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc new file mode 100644 index 0000000..62c92f7 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc @@ -0,0 +1,84 @@ +/* + * 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/relative_pose_cost_3d.h" + +#include "cartographer/pose_graph/internal/testing/test_helpers.h" +#include "gmock/gmock.h" +#include "google/protobuf/text_format.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using ::google::protobuf::TextFormat; +using ::testing::ElementsAre; +using testing::EqualsProto; + +using PositionType = std::array; +using RotationType = std::array; +using ResidualType = std::array; + +::testing::Matcher Near(double expected) { + constexpr double kPrecision = 1e-05; + return ::testing::DoubleNear(expected, kPrecision); +} + +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 +)PROTO"; + +TEST(RelativePoseCost3DTest, SerializesCorrectly) { + proto::RelativePose3D::Parameters relative_pose_parameters; + ASSERT_TRUE( + TextFormat::ParseFromString(kParameters, &relative_pose_parameters)); + RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters); + const auto actual_proto = relative_pose_cost_3d.ToProto(); + EXPECT_THAT(actual_proto, EqualsProto(kParameters)); +} + +TEST(RelativePoseCost3DTest, EvaluatesCorrectly) { + proto::RelativePose3D::Parameters relative_pose_parameters; + ASSERT_TRUE( + TextFormat::ParseFromString(kParameters, &relative_pose_parameters)); + RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters); + + const PositionType kPosition1{{1., 1., 1.}}; + const RotationType kRotation1{{1., 1., 1., 1.}}; + ResidualType residuals; + EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(), + kPosition1.data(), kRotation1.data(), + residuals.data())); + EXPECT_THAT(residuals, ElementsAre(Near(1), Near(2), Near(3), Near(0), + Near(19.1037), Near(6.3679))); + + const PositionType kPosition2{{0., -1., -2.}}; + const RotationType kRotation2{{.1, .2, .3, .4}}; + EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(), + kPosition2.data(), kRotation2.data(), + residuals.data())); + EXPECT_THAT(residuals, ElementsAre(Near(6), Near(8), Near(-2), Near(1.03544), + Near(11.38984), Near(3.10632))); +} + +} // 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 ed43ddf..41ae5b0 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -35,9 +35,12 @@ message RelativePose3D { NodeId first = 1; NodeId second = 2; - transform.proto.Rigid3d first_t_second = 3; - double translation_weight = 4; - double rotation_weight = 5; + message Parameters { + transform.proto.Rigid3d first_t_second = 1; + double translation_weight = 2; + double rotation_weight = 3; + } + Parameters parameters = 3; } message Acceleration3D {