From 3a9d1bc465c6bb6fc92db730b62d1d121f082d9f Mon Sep 17 00:00:00 2001 From: Martin Bokeloh Date: Fri, 20 Jul 2018 17:45:41 +0200 Subject: [PATCH] [GenericPoseGraph] Added rotation cost 3d. (#1313) --- .../cost_function/rotation_cost_3d.cc | 38 +++++++++++ .../cost_function/rotation_cost_3d.h | 63 ++++++++++++++++++ .../cost_function/rotation_cost_3d_test.cc | 64 +++++++++++++++++++ .../pose_graph/proto/cost_function.proto | 6 +- 4 files changed, 169 insertions(+), 2 deletions(-) create mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc create mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h create mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc new file mode 100644 index 0000000..24aa9d8 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc @@ -0,0 +1,38 @@ +/* + * 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/rotation_cost_3d.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace pose_graph { + +RotationCost3D::RotationCost3D(const proto::Rotation3D::Parameters& parameters) + : scaling_factor_(parameters.scaling_factor()), + delta_rotation_imu_frame_( + transform::ToEigen(parameters.delta_rotation_imu_frame())) {} + +proto::Rotation3D::Parameters RotationCost3D::ToProto() const { + proto::Rotation3D::Parameters parameters; + parameters.set_scaling_factor(scaling_factor_); + *parameters.mutable_delta_rotation_imu_frame() = + transform::ToProto(delta_rotation_imu_frame_); + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer \ No newline at end of file diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h new file mode 100644 index 0000000..45842bc --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h @@ -0,0 +1,63 @@ +/* + * 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_ROTATION_COST_3D_H +#define CARTOGRAPHER_ROTATION_COST_3D_H + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/pose_graph/proto/cost_function.pb.h" + +namespace cartographer { +namespace pose_graph { + +// Provides cost function for 3D rotation and de/serialization methods. The cost +// function penalizes differences between IMU data and optimized orientations. +class RotationCost3D { + public: + explicit RotationCost3D(const proto::Rotation3D::Parameters& parameters); + + proto::Rotation3D::Parameters ToProto() const; + + // Cost function expecting three quaternions as input and 3D vector as output. + template + bool operator()(const T* const start_rotation, const T* const end_rotation, + const T* const imu_calibration, T* residual) const { + const Eigen::Quaternion start(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3]); + const Eigen::Quaternion end(end_rotation[0], end_rotation[1], + end_rotation[2], end_rotation[3]); + const Eigen::Quaternion eigen_imu_calibration( + imu_calibration[0], imu_calibration[1], imu_calibration[2], + imu_calibration[3]); + const Eigen::Quaternion error = + end.conjugate() * start * eigen_imu_calibration * + delta_rotation_imu_frame_.cast() * eigen_imu_calibration.conjugate(); + residual[0] = scaling_factor_ * error.x(); + residual[1] = scaling_factor_ * error.y(); + residual[2] = scaling_factor_ * error.z(); + return true; + } + + private: + const double scaling_factor_; + const Eigen::Quaterniond delta_rotation_imu_frame_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_ROTATION_COST_3D_H diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc new file mode 100644 index 0000000..14b6342 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc @@ -0,0 +1,64 @@ +/* + * 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/rotation_cost_3d.h" + +#include "cartographer/pose_graph/internal/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using ::testing::ElementsAre; +using testing::EqualsProto; +using testing::Near; +using testing::ParseProto; + +using RotationType = std::array; +using ResidualType = std::array; + +constexpr char kParameters[] = R"PROTO( + delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 } + scaling_factor: 0.4 +)PROTO"; + +TEST(RotationCost3DTest, SerializesCorrectly) { + const auto parameters = + ParseProto(kParameters); + RotationCost3D rotation_cost_3d(parameters); + const auto actual_proto = rotation_cost_3d.ToProto(); + EXPECT_THAT(actual_proto, EqualsProto(kParameters)); +} + +TEST(RotationCost3DTest, EvaluatesCorrectly) { + const auto parameters = + ParseProto(kParameters); + RotationCost3D rotation_cost_3d(parameters); + + const RotationType kStartRotation{{1., 1., 1., 1.}}; + const RotationType kEndRotation{{1.1, 1.2, 1.3, 1.4}}; + const RotationType kImuCalibration{{0.1, 0.1, 0.1, 0.1}}; + ResidualType residuals; + rotation_cost_3d(kStartRotation.data(), kEndRotation.data(), + kImuCalibration.data(), residuals.data()); + + EXPECT_THAT(residuals, + ElementsAre(Near(0.01536), Near(-0.00256), Near(0.00832))); +} + +} // 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 41ae5b0..24353fa 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -60,8 +60,10 @@ message Rotation3D { NodeId second = 2; NodeId imu_calibration = 3; - transform.proto.Quaterniond delta_rotation_imu_frame = 4; - double scaling_factor = 5; + message Parameters { + transform.proto.Quaterniond delta_rotation_imu_frame = 4; + double scaling_factor = 5; + } } message CostFunction {