diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc new file mode 100644 index 0000000..77076a0 --- /dev/null +++ b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc @@ -0,0 +1,118 @@ +/* + * 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/acceleration_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()); + } +} + +void AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) { + auto gravity = pose->mutable_gravity_constant(); + auto orientation = pose->mutable_orientation(); + problem->AddParameterBlock(gravity, 1); + problem->AddParameterBlock(orientation->data(), orientation->size()); + if (pose->constant()) { + problem->SetParameterBlockConstant(gravity); + problem->SetParameterBlockConstant(orientation->data()); + } +} + +} // namespace + +AccelerationConstraint3D::AccelerationConstraint3D( + const ConstraintId& id, const proto::LossFunction& loss_function_proto, + const proto::Acceleration3D& proto) + : Constraint(id, loss_function_proto), + first_(proto.first()), + second_(proto.second()), + third_(proto.third()), + imu_(proto.imu_calibration()), + cost_(new AccelerationCost3D(proto.parameters())), + ceres_cost_(common::make_unique(cost_)) {} + +void AccelerationConstraint3D::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; + } + + auto third_node = common::FindOrNull(nodes->pose_3d_nodes, third_); + if (third_node == nullptr) { + LOG(INFO) << "Third node was not found in pose_3d_nodes."; + return; + } + + auto imu_node = common::FindOrNull(nodes->imu_calibration_nodes, imu_); + if (imu_node == nullptr) { + LOG(INFO) << "IMU calibration node was not found in imu_calibration_nodes."; + return; + } + + if (first_node->constant() && second_node->constant() && + third_node->constant() && imu_node->constant()) { + LOG(INFO) << "All nodes are constant, skipping the constraint."; + return; + } + + AddPoseParameters(first_node, problem); + AddPoseParameters(second_node, problem); + AddPoseParameters(third_node, problem); + AddImuParameters(imu_node, problem); + problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), + second_node->mutable_rotation()->data(), + second_node->mutable_translation()->data(), + first_node->mutable_translation()->data(), + third_node->mutable_translation()->data(), + imu_node->mutable_gravity_constant(), + imu_node->mutable_orientation()->data()); +} + +proto::CostFunction AccelerationConstraint3D::ToCostFunctionProto() const { + proto::CostFunction cost_function; + auto* acceleration_3d = cost_function.mutable_acceleration_3d(); + *acceleration_3d->mutable_first() = first_.ToProto(); + *acceleration_3d->mutable_second() = second_.ToProto(); + *acceleration_3d->mutable_third() = third_.ToProto(); + *acceleration_3d->mutable_imu_calibration() = imu_.ToProto(); + *acceleration_3d->mutable_parameters() = cost_->ToProto(); + return cost_function; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h b/cartographer/pose_graph/constraint/acceleration_constraint_3d.h new file mode 100644 index 0000000..b39d323 --- /dev/null +++ b/cartographer/pose_graph/constraint/acceleration_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_ACCELERATION_CONSTRAINT_3D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h" + +namespace cartographer { +namespace pose_graph { + +class AccelerationConstraint3D : public Constraint { + public: + AccelerationConstraint3D(const ConstraintId& id, + const proto::LossFunction& loss_function_proto, + const proto::Acceleration3D& proto); + + void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + + protected: + proto::CostFunction ToCostFunctionProto() const final; + + private: + // clang-format off + using AutoDiffFunction = + ceres::AutoDiffCostFunction; + // clang-format on + NodeId first_; + NodeId second_; + NodeId third_; + NodeId imu_; + // The cost function is owned by the ceres cost function. + AccelerationCost3D* const cost_; + std::unique_ptr ceres_cost_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc new file mode 100644 index 0000000..7378df4 --- /dev/null +++ b/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc @@ -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. + */ + +#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.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: "hal_acceleration" + cost_function { + acceleration_3d { + first { object_id: "hal9000" timestamp: 100 } + second { object_id: "hal9000" timestamp: 200 } + third { object_id: "hal9000" timestamp: 300 } + imu_calibration { object_id: "hal_imu" } + parameters { + delta_velocity_imu_frame { x: 1 y: 1 z: 1 } + first_to_second_delta_time_seconds: 10.0 + second_to_third_delta_time_seconds: 20.0 + scaling_factor: 2.0 + } + } + } + loss_function { quadratic_loss {} } +)PROTO"; + +TEST(AccelerationConstraint3DTest, SerializesCorrectly) { + const auto proto = ParseProto(kConstraint); + AccelerationConstraint3D constraint(proto.id(), proto.loss_function(), + proto.cost_function().acceleration_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/constraint/cost_function/acceleration_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc new file mode 100644 index 0000000..4ac374a --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc @@ -0,0 +1,47 @@ +/* + * 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/acceleration_cost_3d.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace pose_graph { + +AccelerationCost3D::AccelerationCost3D( + const proto::Acceleration3D::Parameters& parameters) + : delta_velocity_imu_frame_( + transform::ToEigen(parameters.delta_velocity_imu_frame())), + first_to_second_delta_time_seconds_( + parameters.first_to_second_delta_time_seconds()), + second_to_third_delta_time_seconds_( + parameters.second_to_third_delta_time_seconds()), + scaling_factor_(parameters.scaling_factor()) {} + +proto::Acceleration3D::Parameters AccelerationCost3D::ToProto() const { + proto::Acceleration3D::Parameters parameters; + *parameters.mutable_delta_velocity_imu_frame() = + transform::ToProto(delta_velocity_imu_frame_); + parameters.set_first_to_second_delta_time_seconds( + first_to_second_delta_time_seconds_); + parameters.set_second_to_third_delta_time_seconds( + second_to_third_delta_time_seconds_); + parameters.set_scaling_factor(scaling_factor_); + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h new file mode 100644 index 0000000..e93dc98 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h @@ -0,0 +1,75 @@ +/* + * 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_ACCELERATION_COST_3D_H_ +#define CARTOGRAPHER_ACCELERATION_COST_3D_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/pose_graph/proto/cost_function.pb.h" + +namespace cartographer { +namespace pose_graph { + +// Penalizes differences between IMU data and optimized accelerations. +class AccelerationCost3D { + public: + explicit AccelerationCost3D( + const proto::Acceleration3D::Parameters& parameters); + + proto::Acceleration3D::Parameters ToProto() const; + + template + bool operator()(const T* const middle_rotation, const T* const start_position, + const T* const middle_position, const T* const end_position, + const T* const gravity_constant, + const T* const imu_calibration, T* residual) const { + using Vector3T = Eigen::Matrix; + using TranslationMap = Eigen::Map; + using RotationMap = Eigen::Map>; + + const Vector3T imu_delta_velocity = + RotationMap(middle_rotation) * RotationMap(imu_calibration) * + delta_velocity_imu_frame_.cast() - + (*gravity_constant * 0.5 * + (first_to_second_delta_time_seconds_ + + second_to_third_delta_time_seconds_)) * + Eigen::Vector3d::UnitZ().cast(); + + const Vector3T start_velocity = + (TranslationMap(middle_position) - TranslationMap(start_position)) / + T(first_to_second_delta_time_seconds_); + const Vector3T end_velocity = + (TranslationMap(end_position) - TranslationMap(middle_position)) / + T(second_to_third_delta_time_seconds_); + const Vector3T delta_velocity = end_velocity - start_velocity; + + (Eigen::Map(residual) = + T(scaling_factor_) * (imu_delta_velocity - delta_velocity)); + return true; + } + + private: + const Eigen::Vector3d delta_velocity_imu_frame_; + const double first_to_second_delta_time_seconds_; + const double second_to_third_delta_time_seconds_; + const double scaling_factor_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_ACCELERATION_COST_3D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc new file mode 100644 index 0000000..e7e60e3 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc @@ -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. + */ + +#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h" + +#include "cartographer/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using ::google::protobuf::TextFormat; +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( + delta_velocity_imu_frame { x: 1 y: 1 z: 1 } + first_to_second_delta_time_seconds: 10.0 + second_to_third_delta_time_seconds: 20.0 + scaling_factor: 2.0 +)PROTO"; + +TEST(AccelerationCost3DTest, SerializesCorrectly) { + AccelerationCost3D acceleration_cost_3d( + ParseProto(kParameters)); + EXPECT_THAT(acceleration_cost_3d.ToProto(), EqualsProto(kParameters)); +} + +TEST(AccelerationCost3DTest, EvaluatesCorrectly) { + AccelerationCost3D acceleration_cost_3d( + ParseProto(kParameters)); + const PositionType kStartPosition{{1., 1., 1.}}; + + const PositionType kMiddlePosition{{2., 2., 2.}}; + const RotationType kMiddleRotation{{0., 0., 0., 1.}}; + + const PositionType kEndPosition{{3., 3., 3.}}; + + const double kGravityConstant{10.}; + const RotationType kImuCalibration{{0., 0., 0., 1.0}}; + + ResidualType residuals; + EXPECT_TRUE(acceleration_cost_3d( + kMiddleRotation.data(), kStartPosition.data(), kMiddlePosition.data(), + kEndPosition.data(), &kGravityConstant, kImuCalibration.data(), + residuals.data())); + EXPECT_THAT(residuals, ElementsAre(Near(2.1), Near(2.1), Near(-297.9))); +} + +} // 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 f47f98d..bdd2f87 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -51,8 +51,8 @@ message Acceleration3D { message Parameters { transform.proto.Vector3d delta_velocity_imu_frame = 1; - int64 first_to_second_time_delta = 2; - int64 second_to_third_time_delta = 3; + double first_to_second_delta_time_seconds = 2; + double second_to_third_delta_time_seconds = 3; double scaling_factor = 4; } Parameters parameters = 5;