From ea2bf3b1b2fc9ea00f094ccfca24c4d17f10763a Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 14 Mar 2019 08:12:04 +0100 Subject: [PATCH 01/99] Remove GenericPoseGraph. (#1540) Remove GenericPoseGraph implementation not to confuse the users. It will come back one day. --- .../constraint/acceleration_constraint_3d.cc | 80 ------------ .../constraint/acceleration_constraint_3d.h | 61 --------- .../acceleration_constraint_3d_test.cc | 57 --------- .../pose_graph/constraint/constraint.cc | 32 ----- .../pose_graph/constraint/constraint.h | 63 ---------- .../pose_graph/constraint/constraint_utils.cc | 59 --------- .../pose_graph/constraint/constraint_utils.h | 53 -------- .../cost_function/acceleration_cost_3d.cc | 47 ------- .../cost_function/acceleration_cost_3d.h | 75 ----------- .../acceleration_cost_3d_test.cc | 73 ----------- .../interpolated_relative_pose_cost_2d.cc | 48 ------- .../interpolated_relative_pose_cost_2d.h | 66 ---------- ...interpolated_relative_pose_cost_2d_test.cc | 76 ------------ .../interpolated_relative_pose_cost_3d.cc | 40 ------ .../interpolated_relative_pose_cost_3d.h | 71 ----------- ...interpolated_relative_pose_cost_3d_test.cc | 76 ------------ .../cost_function/relative_pose_cost_2d.cc | 94 -------------- .../cost_function/relative_pose_cost_2d.h | 51 -------- .../relative_pose_cost_2d_test.cc | 117 ------------------ .../cost_function/relative_pose_cost_3d.cc | 37 ------ .../cost_function/relative_pose_cost_3d.h | 57 --------- .../relative_pose_cost_3d_test.cc | 76 ------------ .../cost_function/rotation_cost_3d.cc | 38 ------ .../cost_function/rotation_cost_3d.h | 63 ---------- .../cost_function/rotation_cost_3d_test.cc | 66 ---------- ...nterpolated_relative_pose_constraint_2d.cc | 75 ----------- ...interpolated_relative_pose_constraint_2d.h | 59 --------- ...olated_relative_pose_constraint_2d_test.cc | 62 ---------- ...nterpolated_relative_pose_constraint_3d.cc | 77 ------------ ...interpolated_relative_pose_constraint_3d.h | 61 --------- ...olated_relative_pose_constraint_3d_test.cc | 67 ---------- .../constraint/loss_function/loss_function.cc | 44 ------- .../constraint/loss_function/loss_function.h | 44 ------- .../loss_function/loss_function_test.cc | 45 ------- .../constraint/relative_pose_constraint_2d.cc | 72 ----------- .../constraint/relative_pose_constraint_2d.h | 46 ------- .../relative_pose_constraint_2d_test.cc | 58 --------- .../constraint/relative_pose_constraint_3d.cc | 66 ---------- .../constraint/relative_pose_constraint_3d.h | 55 -------- .../relative_pose_constraint_3d_test.cc | 58 --------- .../constraint/rotation_constraint_3d.cc | 91 -------------- .../constraint/rotation_constraint_3d.h | 52 -------- .../constraint/rotation_constraint_3d_test.cc | 54 -------- .../pose_graph/node/imu_calibration.cc | 50 -------- .../pose_graph/node/imu_calibration.h | 57 --------- .../pose_graph/node/imu_calibration_test.cc | 51 -------- cartographer/pose_graph/node/node.cc | 31 ----- cartographer/pose_graph/node/node.h | 53 -------- cartographer/pose_graph/node/node_id.cc | 42 ------- cartographer/pose_graph/node/node_id.h | 63 ---------- cartographer/pose_graph/node/node_id_test.cc | 62 ---------- cartographer/pose_graph/node/nodes.h | 39 ------ .../node/parameterization/parameterization.cc | 61 --------- .../node/parameterization/parameterization.h | 44 ------- cartographer/pose_graph/node/pose_2d.cc | 47 ------- cartographer/pose_graph/node/pose_2d.h | 46 ------- cartographer/pose_graph/node/pose_2d_test.cc | 49 -------- cartographer/pose_graph/node/pose_3d.cc | 54 -------- cartographer/pose_graph/node/pose_3d.h | 60 --------- cartographer/pose_graph/node/pose_3d_test.cc | 52 -------- .../pose_graph/pose_graph_controller.cc | 48 ------- .../pose_graph/pose_graph_controller.h | 53 -------- cartographer/pose_graph/pose_graph_data.cc | 100 --------------- cartographer/pose_graph/pose_graph_data.h | 38 ------ .../pose_graph/proto/constraint.proto | 26 ---- .../pose_graph/proto/cost_function.proto | 114 ----------------- .../pose_graph/proto/loss_function.proto | 30 ----- cartographer/pose_graph/proto/node.proto | 65 ---------- .../pose_graph/proto/pose_graph_data.proto | 25 ---- .../pose_graph/proto/solver_config.proto | 14 --- .../pose_graph/solver/ceres_solver.cc | 64 ---------- cartographer/pose_graph/solver/ceres_solver.h | 41 ------ .../pose_graph/solver/ceres_solver_test.cc | 82 ------------ cartographer/pose_graph/solver/solver.h | 45 ------- 74 files changed, 4268 deletions(-) delete mode 100644 cartographer/pose_graph/constraint/acceleration_constraint_3d.cc delete mode 100644 cartographer/pose_graph/constraint/acceleration_constraint_3d.h delete mode 100644 cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/constraint.cc delete mode 100644 cartographer/pose_graph/constraint/constraint.h delete mode 100644 cartographer/pose_graph/constraint/constraint_utils.cc delete mode 100644 cartographer/pose_graph/constraint/constraint_utils.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc delete mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h delete mode 100644 cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h delete mode 100644 cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/loss_function/loss_function.cc delete mode 100644 cartographer/pose_graph/constraint/loss_function/loss_function.h delete mode 100644 cartographer/pose_graph/constraint/loss_function/loss_function_test.cc delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_2d.h delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_3d.h delete mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc delete mode 100644 cartographer/pose_graph/constraint/rotation_constraint_3d.cc delete mode 100644 cartographer/pose_graph/constraint/rotation_constraint_3d.h delete mode 100644 cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc delete mode 100644 cartographer/pose_graph/node/imu_calibration.cc delete mode 100644 cartographer/pose_graph/node/imu_calibration.h delete mode 100644 cartographer/pose_graph/node/imu_calibration_test.cc delete mode 100644 cartographer/pose_graph/node/node.cc delete mode 100644 cartographer/pose_graph/node/node.h delete mode 100644 cartographer/pose_graph/node/node_id.cc delete mode 100644 cartographer/pose_graph/node/node_id.h delete mode 100644 cartographer/pose_graph/node/node_id_test.cc delete mode 100644 cartographer/pose_graph/node/nodes.h delete mode 100644 cartographer/pose_graph/node/parameterization/parameterization.cc delete mode 100644 cartographer/pose_graph/node/parameterization/parameterization.h delete mode 100644 cartographer/pose_graph/node/pose_2d.cc delete mode 100644 cartographer/pose_graph/node/pose_2d.h delete mode 100644 cartographer/pose_graph/node/pose_2d_test.cc delete mode 100644 cartographer/pose_graph/node/pose_3d.cc delete mode 100644 cartographer/pose_graph/node/pose_3d.h delete mode 100644 cartographer/pose_graph/node/pose_3d_test.cc delete mode 100644 cartographer/pose_graph/pose_graph_controller.cc delete mode 100644 cartographer/pose_graph/pose_graph_controller.h delete mode 100644 cartographer/pose_graph/pose_graph_data.cc delete mode 100644 cartographer/pose_graph/pose_graph_data.h delete mode 100644 cartographer/pose_graph/proto/constraint.proto delete mode 100644 cartographer/pose_graph/proto/cost_function.proto delete mode 100644 cartographer/pose_graph/proto/loss_function.proto delete mode 100644 cartographer/pose_graph/proto/node.proto delete mode 100644 cartographer/pose_graph/proto/pose_graph_data.proto delete mode 100644 cartographer/pose_graph/proto/solver_config.proto delete mode 100644 cartographer/pose_graph/solver/ceres_solver.cc delete mode 100644 cartographer/pose_graph/solver/ceres_solver.h delete mode 100644 cartographer/pose_graph/solver/ceres_solver_test.cc delete mode 100644 cartographer/pose_graph/solver/solver.h diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc deleted file mode 100644 index 87fbf6a27c..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc +++ /dev/null @@ -1,80 +0,0 @@ -/* - * 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 "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -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_(absl::make_unique(cost_)) {} - -void AccelerationConstraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(third_node, third_, nodes->pose_3d_nodes, - "Third node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN( - imu_node, imu_, nodes->imu_calibration_nodes, - "IMU calibration node was not found in imu_calibration_nodes."); - - if (first_node->constant() && second_node->constant() && - third_node->constant() && imu_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node, problem); - AddPose3D(second_node, problem); - AddPose3D(third_node, problem); - AddImuCalibration(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 deleted file mode 100644 index 0321a83497..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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 AddToSolver(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_ACCELERATION_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 deleted file mode 100644 index 7378df45c1..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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/constraint.cc b/cartographer/pose_graph/constraint/constraint.cc deleted file mode 100644 index b6dce5a5a4..0000000000 --- a/cartographer/pose_graph/constraint/constraint.cc +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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/constraint.h" - -namespace cartographer { -namespace pose_graph { - -// TODO(pifon): Add a test. -proto::Constraint Constraint::ToProto() const { - proto::Constraint constraint; - constraint.set_id(constraint_id_); - *constraint.mutable_cost_function() = ToCostFunctionProto(); - *constraint.mutable_loss_function() = loss_function_.ToProto(); - return constraint; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint.h b/cartographer/pose_graph/constraint/constraint.h deleted file mode 100644 index 0fcda85dcd..0000000000 --- a/cartographer/pose_graph/constraint/constraint.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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_CONSTRAINT_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ - -#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" -#include "cartographer/pose_graph/node/nodes.h" -#include "cartographer/pose_graph/proto/constraint.pb.h" -#include "ceres/problem.h" - -#include - -namespace cartographer { -namespace pose_graph { - -using ConstraintId = std::string; - -class Constraint { - public: - Constraint(const ConstraintId& id, - const proto::LossFunction& loss_function_proto) - : constraint_id_(id), loss_function_(loss_function_proto) {} - virtual ~Constraint() = default; - - Constraint(const Constraint&) = delete; - Constraint& operator=(const Constraint&) = delete; - - proto::Constraint ToProto() const; - - const ConstraintId& constraint_id() const { return constraint_id_; } - - virtual void AddToSolver(Nodes* nodes, ceres::Problem* problem) const = 0; - - protected: - virtual proto::CostFunction ToCostFunctionProto() const = 0; - - ceres::LossFunction* ceres_loss() const { - return loss_function_.ceres_loss(); - } - - private: - ConstraintId constraint_id_; - LossFunction loss_function_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ diff --git a/cartographer/pose_graph/constraint/constraint_utils.cc b/cartographer/pose_graph/constraint/constraint_utils.cc deleted file mode 100644 index 89e5dffa1d..0000000000 --- a/cartographer/pose_graph/constraint/constraint_utils.cc +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -void AddPose2D(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()); - } -} - -void AddPose3D(Pose3D* pose, ceres::Problem* problem) { - auto transation = pose->mutable_translation(); - auto rotation = pose->mutable_rotation(); - - problem->AddParameterBlock(transation->data(), transation->size(), - pose->translation_parameterization()); - problem->AddParameterBlock(rotation->data(), rotation->size(), - pose->rotation_parameterization()); - if (pose->constant()) { - problem->SetParameterBlockConstant(transation->data()); - problem->SetParameterBlockConstant(rotation->data()); - } -} - -void AddImuCalibration(ImuCalibration* imu_calibration, - ceres::Problem* problem) { - auto gravity = imu_calibration->mutable_gravity_constant(); - auto orientation = imu_calibration->mutable_orientation(); - - problem->AddParameterBlock(gravity, 1); - problem->AddParameterBlock(orientation->data(), orientation->size(), - imu_calibration->orientation_parameterization()); - if (imu_calibration->constant()) { - problem->SetParameterBlockConstant(gravity); - problem->SetParameterBlockConstant(orientation->data()); - } -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint_utils.h b/cartographer/pose_graph/constraint/constraint_utils.h deleted file mode 100644 index 67cbaed79e..0000000000 --- a/cartographer/pose_graph/constraint/constraint_utils.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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_CONSTRAINT_UTILS_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ - -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/node/imu_calibration.h" -#include "cartographer/pose_graph/node/pose_2d.h" -#include "cartographer/pose_graph/node/pose_3d.h" -#include "ceres/problem.h" - -namespace cartographer { -namespace pose_graph { - -void AddPose2D(Pose2D* pose, ceres::Problem* problem); - -void AddPose3D(Pose3D* pose, ceres::Problem* problem); - -void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem); - -template -ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) { - auto node = common::FindOrNull(map, node_id); - return node == nullptr ? nullptr : node->get(); -} - -#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \ - auto node_name = FindNodeOrNull(map, node_id); \ - if (node_name == nullptr) { \ - LOG(INFO) << log_message; \ - return; \ - } - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc deleted file mode 100644 index 4ac374a595..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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 deleted file mode 100644 index e93dc98f4d..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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 deleted file mode 100644 index fcb464577f..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc +++ /dev/null @@ -1,73 +0,0 @@ -/* - * 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 - -#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/constraint/cost_function/interpolated_relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc deleted file mode 100644 index b09c74e478..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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 deleted file mode 100644 index e86dfb5ca0..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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 deleted file mode 100644 index 7464e10774..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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/cost_function/interpolated_relative_pose_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc deleted file mode 100644 index 5e3003527c..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc +++ /dev/null @@ -1,40 +0,0 @@ -/* - * 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 deleted file mode 100644 index dab6c9c0aa..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * 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 deleted file mode 100644 index f5e88a08b7..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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/cost_function/relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc deleted file mode 100644 index fae18cc425..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc +++ /dev/null @@ -1,94 +0,0 @@ -/* - * 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_2d.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseCost2D::RelativePoseCost2D( - const proto::RelativePose2D::Parameters& parameters) - : translation_weight_(parameters.translation_weight()), - rotation_weight_(parameters.rotation_weight()), - first_T_second_(transform::ToRigid2(parameters.first_t_second())) {} - -proto::RelativePose2D::Parameters RelativePoseCost2D::ToProto() const { - proto::RelativePose2D::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; -} - -bool RelativePoseCost2D::Evaluate(double const* const* parameters, - double* residuals, double** jacobians) const { - double const* start = parameters[0]; - double const* end = parameters[1]; - - const double cos_start_rotation = cos(start[2]); - const double sin_start_rotation = sin(start[2]); - const double delta_x = end[0] - start[0]; - const double delta_y = end[1] - start[1]; - - residuals[0] = - translation_weight_ * - (first_T_second_.translation().x() - - (cos_start_rotation * delta_x + sin_start_rotation * delta_y)); - residuals[1] = - translation_weight_ * - (first_T_second_.translation().y() - - (-sin_start_rotation * delta_x + cos_start_rotation * delta_y)); - residuals[2] = rotation_weight_ * - common::NormalizeAngleDifference( - first_T_second_.rotation().angle() - (end[2] - start[2])); - if (jacobians == nullptr) return true; - - const double weighted_cos_start_rotation = - translation_weight_ * cos_start_rotation; - const double weighted_sin_start_rotation = - translation_weight_ * sin_start_rotation; - - // Jacobians in Ceres are ordered by the parameter blocks: - // jacobian[i] = [(dr_0 / dx_i)^T, ..., (dr_n / dx_i)^T]. - if (jacobians[0] != nullptr) { - jacobians[0][0] = weighted_cos_start_rotation; - jacobians[0][1] = weighted_sin_start_rotation; - jacobians[0][2] = weighted_sin_start_rotation * delta_x - - weighted_cos_start_rotation * delta_y; - jacobians[0][3] = -weighted_sin_start_rotation; - jacobians[0][4] = weighted_cos_start_rotation; - jacobians[0][5] = weighted_cos_start_rotation * delta_x + - weighted_sin_start_rotation * delta_y; - jacobians[0][6] = 0; - jacobians[0][7] = 0; - jacobians[0][8] = rotation_weight_; - } - if (jacobians[1] != nullptr) { - jacobians[1][0] = -weighted_cos_start_rotation; - jacobians[1][1] = -weighted_sin_start_rotation; - jacobians[1][2] = 0; - jacobians[1][3] = weighted_sin_start_rotation; - jacobians[1][4] = -weighted_cos_start_rotation; - jacobians[1][5] = 0; - jacobians[1][6] = 0; - jacobians[1][7] = 0; - jacobians[1][8] = -rotation_weight_; - } - return true; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h deleted file mode 100644 index 5395e89862..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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_RELATIVE_POSE_COST_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_ - -#include "cartographer/pose_graph/proto/cost_function.pb.h" -#include "cartographer/transform/transform.h" -#include "ceres/sized_cost_function.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseCost2D - : public ceres::SizedCostFunction<3 /* number of residuals */, - 3 /* size of first pose */, - 3 /* size of second pose */> { - public: - explicit RelativePoseCost2D( - const proto::RelativePose2D::Parameters& parameters); - - proto::RelativePose2D::Parameters ToProto() const; - - // Parameters are packed as [first_pose_2d, second_pose_2d], where each 2D - // pose is [translation_x, translation_y, rotation]. - bool Evaluate(double const* const* parameters, double* residuals, - double** jacobians) const final; - - private: - const double translation_weight_; - const double rotation_weight_; - const transform::Rigid2d first_T_second_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc deleted file mode 100644 index cf1a23fd4c..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc +++ /dev/null @@ -1,117 +0,0 @@ -/* - * 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_2d.h" - -#include "absl/memory/memory.h" -#include "cartographer/testing/test_helpers.h" -#include "ceres/gradient_checker.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -constexpr int kPoseDimension = 3; -constexpr int kResidualsCount = 3; -constexpr int kParameterBlocksCount = 2; -constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension; - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using ResidualType = std::array; -using JacobianType = std::array, - kParameterBlocksCount>; - -constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 1 } - rotation: -2.214297 - } - translation_weight: 1 - rotation_weight: 10 -)PROTO"; - -class RelativePoseCost2DTest : public ::testing::Test { - public: - RelativePoseCost2DTest() - : relative_pose_cost_2d_(absl::make_unique( - ParseProto(kParameters))) { - for (int i = 0; i < kParameterBlocksCount; ++i) { - jacobian_ptrs_[i] = jacobian_[i].data(); - } - } - - std::pair - EvaluateRelativePoseCost2D( - const std::array& parameter_blocks) { - relative_pose_cost_2d_->Evaluate(parameter_blocks.data(), residuals_.data(), - jacobian_ptrs_.data()); - return std::make_pair(std::cref(residuals_), std::cref(jacobian_)); - } - - protected: - ResidualType residuals_; - JacobianType jacobian_; - std::array jacobian_ptrs_; - std::unique_ptr relative_pose_cost_2d_; -}; - -TEST_F(RelativePoseCost2DTest, SerializesCorrectly) { - EXPECT_THAT(relative_pose_cost_2d_->ToProto(), EqualsProto(kParameters)); -} - -TEST_F(RelativePoseCost2DTest, CheckGradient) { - std::array start_pose{{1., 1., 1.}}; - std::array end_pose{{10., 1., 100.}}; - std::array parameter_blocks{ - {start_pose.data(), end_pose.data()}}; - - using ::ceres::GradientChecker; - GradientChecker gradient_checker(relative_pose_cost_2d_.get(), - {} /* local parameterizations */, - ceres::NumericDiffOptions{}); - - GradientChecker::ProbeResults probe_results; - gradient_checker.Probe(parameter_blocks.data(), - 1e-08 /* relative precision */, &probe_results); - EXPECT_TRUE(probe_results.return_value); -} - -TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) { - std::array start_pose{{1., 1., 1.}}; - std::array end_pose{{10., 1., 100.}}; - std::array parameter_blocks{ - {start_pose.data(), end_pose.data()}}; - - auto residuals_and_jacobian = EvaluateRelativePoseCost2D(parameter_blocks); - EXPECT_THAT(residuals_and_jacobian.first, - ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333))); - EXPECT_THAT( - residuals_and_jacobian.second, - ElementsAre( - ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324), - Near(-0.841471), Near(0.540302), Near(4.86272), Near(0), - Near(0), Near(10)), - ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471), - Near(-0.540302), Near(0), Near(0), Near(0), Near(-10)))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer 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 deleted file mode 100644 index 9293fd9fb9..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc +++ /dev/null @@ -1,37 +0,0 @@ -/* - * 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 deleted file mode 100644 index 9c68a7e245..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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 deleted file mode 100644 index d44ad61fba..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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/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 -)PROTO"; - -TEST(RelativePoseCost3DTest, SerializesCorrectly) { - auto relative_pose_parameters = - ParseProto(kParameters); - 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) { - auto relative_pose_parameters = - ParseProto(kParameters); - 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/constraint/cost_function/rotation_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc deleted file mode 100644 index 24aa9d8363..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc +++ /dev/null @@ -1,38 +0,0 @@ -/* - * 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 deleted file mode 100644 index 45842bcd81..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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 deleted file mode 100644 index ca6d1ea3bf..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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 - -#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 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/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc deleted file mode 100644 index 7e8154b23f..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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 "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -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_(absl::make_unique(cost_)) {} - -void InterpolatedRelativePoseConstraint2D::AddToSolver( - Nodes* nodes, ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes, - "First node (start) was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_2d_nodes, - "First node (end) was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node_start->constant() && first_node_end->constant() && - second_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose2D(first_node_start, problem); - AddPose2D(first_node_end, problem); - AddPose3D(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 deleted file mode 100644 index 5635334c9a..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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 AddToSolver(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 deleted file mode 100644 index 8659876bc3..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* - * 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/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc deleted file mode 100644 index 73f31fa02d..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc +++ /dev/null @@ -1,77 +0,0 @@ -/* - * 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 "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -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_(absl::make_unique(cost_)) {} - -void InterpolatedRelativePoseConstraint3D::AddToSolver( - Nodes* nodes, ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes, - "First node (start) was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_3d_nodes, - "First node (end) was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node_start->constant() && first_node_end->constant() && - second_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node_start, problem); - AddPose3D(first_node_end, problem); - AddPose3D(second_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - 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 deleted file mode 100644 index d836561e73..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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 AddToSolver(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 deleted file mode 100644 index 642188af0c..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc +++ /dev/null @@ -1,67 +0,0 @@ -/* - * 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/constraint/loss_function/loss_function.cc b/cartographer/pose_graph/constraint/loss_function/loss_function.cc deleted file mode 100644 index 6620520b0d..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function.cc +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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/loss_function/loss_function.h" - -#include "absl/memory/memory.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -std::unique_ptr CeresLossFromProto( - const proto::LossFunction& proto) { - switch (proto.Type_case()) { - case proto::LossFunction::kHuberLoss: - return absl::make_unique(proto.huber_loss().scale()); - case proto::LossFunction::kQuadraticLoss: - return nullptr; - default: - LOG(FATAL) << "The loss function is not specified."; - return nullptr; - } -} - -} // namespace - -LossFunction::LossFunction(const proto::LossFunction& proto) - : proto_(proto), ceres_loss_(CeresLossFromProto(proto_)) {} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function.h b/cartographer/pose_graph/constraint/loss_function/loss_function.h deleted file mode 100644 index 5cbd53338a..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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_LOSS_FUNCTION_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ - -#include - -#include "cartographer/pose_graph/proto/loss_function.pb.h" -#include "ceres/loss_function.h" - -namespace cartographer { -namespace pose_graph { - -class LossFunction { - public: - explicit LossFunction(const proto::LossFunction& proto); - - const proto::LossFunction& ToProto() const { return proto_; } - - ceres::LossFunction* ceres_loss() const { return ceres_loss_.get(); } - - private: - const proto::LossFunction proto_; - const std::unique_ptr ceres_loss_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc b/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc deleted file mode 100644 index 9f29be1eaa..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc +++ /dev/null @@ -1,45 +0,0 @@ -/* - * 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/loss_function/loss_function.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -TEST(LossFunctionTest, ConstructQuadraticLoss) { - LossFunction quadratic_loss( - ParseProto(R"(quadratic_loss: {})")); - EXPECT_EQ(nullptr, quadratic_loss.ceres_loss()); -} - -TEST(LossFunctionTest, ConstructHuberLoss) { - LossFunction huber_loss( - ParseProto(R"(huber_loss: { scale: 0.5 })")); - EXPECT_NE(nullptr, dynamic_cast(huber_loss.ceres_loss())); -} - -TEST(LossFunctionDeathTest, FailToConstructUnspecifiedLoss) { - EXPECT_DEATH(LossFunction(proto::LossFunction{}), ""); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc deleted file mode 100644 index 46f8264365..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc +++ /dev/null @@ -1,72 +0,0 @@ -/* - * 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/relative_pose_constraint_2d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseConstraint2D::RelativePoseConstraint2D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::RelativePose2D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - ceres_cost_(absl::make_unique(proto.parameters())) {} - -// TODO(pifon): Add a test. -void RelativePoseConstraint2D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes, - "First node was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes, - "Second node was not found in pose_2d_nodes."); - - if (first_node->constant() && second_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - auto first_pose = first_node->mutable_pose_2d(); - problem->AddParameterBlock(first_pose->data(), first_pose->size()); - if (first_node->constant()) { - problem->SetParameterBlockConstant(first_pose->data()); - } - - auto second_pose = second_node->mutable_pose_2d(); - problem->AddParameterBlock(second_pose->data(), second_pose->size()); - if (second_node->constant()) { - problem->SetParameterBlockConstant(second_pose->data()); - } - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), first_pose->data(), - second_pose->data()); -} - -proto::CostFunction RelativePoseConstraint2D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* relative_pose_2d = cost_function.mutable_relative_pose_2d(); - *relative_pose_2d->mutable_first() = first_.ToProto(); - *relative_pose_2d->mutable_second() = second_.ToProto(); - *relative_pose_2d->mutable_parameters() = ceres_cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h deleted file mode 100644 index 23c0bff474..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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_RELATIVE_POSE_CONSTRAINT_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseConstraint2D : public Constraint { - public: - RelativePoseConstraint2D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::RelativePose2D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - NodeId first_; - NodeId second_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc deleted file mode 100644 index 85c396e5d2..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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/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 { - relative_pose_2d { - first { object_id: "node0" } - second { object_id: "node1" } - parameters { - first_t_second { - translation: { x: 1 y: 1 } - rotation: -2.214297 - } - translation_weight: 1 - rotation_weight: 10 - } - } - } - - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RelativePoseConstraint2DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RelativePoseConstraint2D constraint(proto.id(), proto.loss_function(), - proto.cost_function().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/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc deleted file mode 100644 index e688118e69..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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/relative_pose_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseConstraint3D::RelativePoseConstraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::RelativePose3D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - cost_(new RelativePoseCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void RelativePoseConstraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node->constant() && second_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node, problem); - AddPose3D(second_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node->mutable_translation()->data(), - first_node->mutable_rotation()->data(), - second_node->mutable_translation()->data(), - second_node->mutable_rotation()->data()); -} - -proto::CostFunction RelativePoseConstraint3D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* relative_pose_3d = cost_function.mutable_relative_pose_3d(); - *relative_pose_3d->mutable_first() = first_.ToProto(); - *relative_pose_3d->mutable_second() = second_.ToProto(); - *relative_pose_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h deleted file mode 100644 index aadc066eba..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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_RELATIVE_POSE_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseConstraint3D : public Constraint { - public: - RelativePoseConstraint3D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::RelativePose3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - using AutoDiffFunction = - ceres::AutoDiffCostFunction; - NodeId first_; - NodeId second_; - // The cost function is owned by the ceres cost function. - RelativePoseCost3D* 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/relative_pose_constraint_3d_test.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc deleted file mode 100644 index 6af981be78..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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/relative_pose_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: "narf" - cost_function { - relative_pose_3d { - first { object_id: "node0" } - 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: 1 - rotation_weight: 10 - } - } - } - - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RelativePoseConstraint2DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RelativePoseConstraint3D constraint(proto.id(), proto.loss_function(), - proto.cost_function().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/constraint/rotation_constraint_3d.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc deleted file mode 100644 index b9bf6ee909..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc +++ /dev/null @@ -1,91 +0,0 @@ -/* - * 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/rotation_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -#include "cartographer/common/utils.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -void AddRotation3D(Pose3D* pose, ceres::Problem* problem) { - auto rotation = pose->mutable_rotation(); - problem->AddParameterBlock(rotation->data(), rotation->size()); - if (pose->constant()) { - problem->SetParameterBlockConstant(rotation->data()); - } -} - -void AddImuOrientation(ImuCalibration* imu_node, ceres::Problem* problem) { - auto imu_orientation = imu_node->mutable_orientation(); - problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size()); - if (imu_node->constant()) { - problem->SetParameterBlockConstant(imu_orientation->data()); - } -} - -} // namespace - -RotationContraint3D::RotationContraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::Rotation3D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - imu_calibration_(proto.imu_calibration()), - cost_(new RotationCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void RotationContraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(imu_node, imu_calibration_, nodes->imu_calibration_nodes, - "Imu calibration node was not found."); - - if (first_node->constant() && second_node->constant() && - imu_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - AddRotation3D(first_node, problem); - AddRotation3D(second_node, problem); - AddImuOrientation(imu_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node->mutable_rotation()->data(), - second_node->mutable_rotation()->data(), - imu_node->mutable_orientation()->data()); -} - -proto::CostFunction RotationContraint3D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* rotation_3d = cost_function.mutable_rotation_3d(); - *rotation_3d->mutable_first() = first_.ToProto(); - *rotation_3d->mutable_second() = second_.ToProto(); - *rotation_3d->mutable_imu_calibration() = imu_calibration_.ToProto(); - *rotation_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.h b/cartographer/pose_graph/constraint/rotation_constraint_3d.h deleted file mode 100644 index b0dbe24298..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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_ROTATION_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class RotationContraint3D : public Constraint { - public: - RotationContraint3D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::Rotation3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - using AutoDiffFunction = ceres::AutoDiffCostFunction< - RotationCost3D, 3 /* number of residuals */, 4 /* rotation first pose */, - 4 /* rotation second pose */, 4 /* imu calibration */>; - NodeId first_; - NodeId second_; - NodeId imu_calibration_; - // The cost function is owned by the ceres cost function. - RotationCost3D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc deleted file mode 100644 index 0c2d8615c7..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc +++ /dev/null @@ -1,54 +0,0 @@ -/* - * 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/rotation_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: "narf" - cost_function { - rotation_3d { - first { object_id: "node0" } - second { object_id: "node1" } - imu_calibration { object_id: "imu_node" } - parameters { - delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 } - scaling_factor: 0.4 - } - } - } - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RotationConstraint3DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RotationContraint3D constraint(proto.id(), proto.loss_function(), - proto.cost_function().rotation_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/node/imu_calibration.cc b/cartographer/pose_graph/node/imu_calibration.cc deleted file mode 100644 index 3389c7db61..0000000000 --- a/cartographer/pose_graph/node/imu_calibration.cc +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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/node/imu_calibration.h" - -namespace cartographer { -namespace pose_graph { - -ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant, - const proto::ImuCalibration& imu) - : Node(node_id, constant), - gravity_constant_(imu.gravity_constant()), - orientation_{{imu.orientation().x(), imu.orientation().y(), - imu.orientation().z(), imu.orientation().w()}}, - orientation_parameterization_(imu.orientation_parameterization()) {} - -proto::Parameters ImuCalibration::ToParametersProto() const { - proto::Parameters parameters; - auto* imu_calibration = parameters.mutable_imu_calibration(); - - imu_calibration->set_gravity_constant(gravity_constant_); - - // TODO(pifon): Use a common method to convert from Eigen::Quaterniond to - // proto. Probably, the one defined in transform.h. - auto* orientation = imu_calibration->mutable_orientation(); - orientation->set_x(orientation_[0]); - orientation->set_y(orientation_[1]); - orientation->set_z(orientation_[2]); - orientation->set_w(orientation_[3]); - imu_calibration->set_orientation_parameterization( - orientation_parameterization_.ToProto()); - - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/imu_calibration.h b/cartographer/pose_graph/node/imu_calibration.h deleted file mode 100644 index 6febd5b959..0000000000 --- a/cartographer/pose_graph/node/imu_calibration.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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_NODE_IMU_CALIBRATION_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "cartographer/pose_graph/node/parameterization/parameterization.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -class ImuCalibration : public Node { - public: - ImuCalibration(const NodeId& node_id, bool constant, - const proto::ImuCalibration& imu_calibration); - - double* mutable_gravity_constant() { return &gravity_constant_; } - double gravity_constant() const { return gravity_constant_; } - - std::array* mutable_orientation() { return &orientation_; } - const std::array& orientation() const { return orientation_; } - - ceres::LocalParameterization* orientation_parameterization() const { - return orientation_parameterization_.ceres_parameterization(); - } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - double gravity_constant_; - std::array orientation_; - Parameterization orientation_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ diff --git a/cartographer/pose_graph/node/imu_calibration_test.cc b/cartographer/pose_graph/node/imu_calibration_test.cc deleted file mode 100644 index 1894043b1c..0000000000 --- a/cartographer/pose_graph/node/imu_calibration_test.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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/node/imu_calibration.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "accelerometer" timestamp: 1 } - constant: true - parameters { - imu_calibration { - gravity_constant: 10 - orientation: { w: 0 x: 1 y: 2 z: 3 } - orientation_parameterization: YAW_ONLY - } - } -)PROTO"; - -TEST(Pose3DTest, ToProto) { - ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)}, - true, ParseProto(R"( - gravity_constant: 10 - orientation: { w: 0 x: 1 y: 2 z: 3 } - orientation_parameterization: YAW_ONLY - )")); - EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/node.cc b/cartographer/pose_graph/node/node.cc deleted file mode 100644 index b9293f1bd5..0000000000 --- a/cartographer/pose_graph/node/node.cc +++ /dev/null @@ -1,31 +0,0 @@ -/* - * 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/node/node.h" - -namespace cartographer { -namespace pose_graph { - -proto::Node Node::ToProto() const { - proto::Node node; - node.set_constant(constant_); - *node.mutable_id() = node_id_.ToProto(); - *node.mutable_parameters() = ToParametersProto(); - return node; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/node.h b/cartographer/pose_graph/node/node.h deleted file mode 100644 index 8e29ea6a42..0000000000 --- a/cartographer/pose_graph/node/node.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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_NODE_NODE_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ - -#include "cartographer/pose_graph/node/node_id.h" -#include "cartographer/pose_graph/proto/node.pb.h" - -#include - -namespace cartographer { -namespace pose_graph { - -class Node { - public: - explicit Node(const NodeId& id, bool constant) - : node_id_(id), constant_(constant) {} - - ~Node() = default; - - proto::Node ToProto() const; - - const NodeId node_id() const { return node_id_; } - - bool constant() const { return constant_; } - void set_constant(bool constant) { constant_ = constant; } - - protected: - virtual proto::Parameters ToParametersProto() const = 0; - - private: - NodeId node_id_; - bool constant_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ diff --git a/cartographer/pose_graph/node/node_id.cc b/cartographer/pose_graph/node/node_id.cc deleted file mode 100644 index b242b333fe..0000000000 --- a/cartographer/pose_graph/node/node_id.cc +++ /dev/null @@ -1,42 +0,0 @@ -/* - * 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/node/node_id.h" - -namespace cartographer { -namespace pose_graph { - -NodeId::NodeId(const std::string& object_id, common::Time time) - : object_id(object_id), time(time) {} - -NodeId::NodeId(const std::string& object_id, const std::string& group_id, - common::Time time) - : object_id(object_id), group_id(group_id), time(time) {} - -NodeId::NodeId(const proto::NodeId& node_id) - : NodeId(node_id.object_id(), node_id.group_id(), - common::FromUniversal(node_id.timestamp())) {} - -proto::NodeId NodeId::ToProto() const { - proto::NodeId node_id; - node_id.set_object_id(object_id); - node_id.set_group_id(group_id); - node_id.set_timestamp(common::ToUniversal(time)); - return node_id; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/node_id.h b/cartographer/pose_graph/node/node_id.h deleted file mode 100644 index ecb62b1ae4..0000000000 --- a/cartographer/pose_graph/node/node_id.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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_NODE_NODE_ID_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_ - -#include -#include - -#include "cartographer/common/time.h" -#include "cartographer/pose_graph/proto/node.pb.h" - -namespace cartographer { -namespace pose_graph { - -struct NodeId { - NodeId(const std::string& object_id, common::Time time); - NodeId(const std::string& object_id, const std::string& group_id, - common::Time time); - explicit NodeId(const proto::NodeId& node_id); - - // Object refers to dynamic/static objects, e.g. robot/landmark/submap poses, - // IMU calibration, etc. - std::string object_id; - // Id of the group to which the node belongs, e.g. "submap". - std::string group_id; - // Time associated with the object's pose. - common::Time time; - - proto::NodeId ToProto() const; -}; - -inline bool operator<(const NodeId& lhs, const NodeId& rhs) { - return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) < - std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time); -} - -inline std::ostream& operator<<(std::ostream& os, const NodeId& id) { - std::string group_message; - if (!id.group_id.empty()) { - group_message = ", group_id: " + id.group_id; - } - return os << "(object_id: " << id.object_id << group_message - << ", time: " << id.time << ")"; -} - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_ diff --git a/cartographer/pose_graph/node/node_id_test.cc b/cartographer/pose_graph/node/node_id_test.cc deleted file mode 100644 index 37a5a18f68..0000000000 --- a/cartographer/pose_graph/node/node_id_test.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* - * 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/node/node_id.h" - -#include - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -TEST(NodeIdTest, FromProto) { - proto::NodeId proto; - proto.set_object_id("some_object"); - proto.set_group_id("submap"); - proto.set_timestamp(1); - - NodeId node_id(proto); - EXPECT_EQ(node_id.object_id, "some_object"); - EXPECT_EQ(node_id.group_id, "submap"); - EXPECT_EQ(common::ToUniversal(node_id.time), 1); -} - -TEST(NodeIdTest, ToProto) { - NodeId node_id{"some_object", "submap", common::FromUniversal(1)}; - EXPECT_THAT(node_id.ToProto(), - testing::EqualsProto( - "object_id: 'some_object' group_id: 'submap' timestamp: 1")); -} - -TEST(NodeIdTest, ToStringWithoutGroupId) { - std::stringstream ss; - ss << NodeId{"some_object", common::FromUniversal(1)}; - - EXPECT_EQ("(object_id: some_object, time: 1)", ss.str()); -} - -TEST(NodeIdTest, ToStringWithGroupId) { - std::stringstream ss; - ss << NodeId{"some_object", "submap", common::FromUniversal(1)}; - - EXPECT_EQ("(object_id: some_object, group_id: submap, time: 1)", ss.str()); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/nodes.h b/cartographer/pose_graph/node/nodes.h deleted file mode 100644 index 21f98602be..0000000000 --- a/cartographer/pose_graph/node/nodes.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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_NODE_NODES_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ - -#include "cartographer/pose_graph/node/imu_calibration.h" -#include "cartographer/pose_graph/node/pose_2d.h" -#include "cartographer/pose_graph/node/pose_3d.h" - -#include - -namespace cartographer { -namespace pose_graph { - -struct Nodes { - // TODO(pifon): Migrate to Swiss Tables when they are out. - std::map> pose_2d_nodes; - std::map> pose_3d_nodes; - std::map> imu_calibration_nodes; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ diff --git a/cartographer/pose_graph/node/parameterization/parameterization.cc b/cartographer/pose_graph/node/parameterization/parameterization.cc deleted file mode 100644 index 44a5be4529..0000000000 --- a/cartographer/pose_graph/node/parameterization/parameterization.cc +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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/node/parameterization/parameterization.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/math.h" -#include "cartographer/mapping/internal/3d/rotation_parameterization.h" -#include "ceres/autodiff_local_parameterization.h" -#include "ceres/rotation.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using absl::make_unique; -using ceres::AutoDiffLocalParameterization; -using ceres::LocalParameterization; - -// TODO(pifon): Check if the functors are compatible with our quaternions. Test! -std::unique_ptr CeresParameterizationFromProto( - const proto::Parameterization& parameterization) { - switch (parameterization) { - case (proto::Parameterization::NONE): - return nullptr; - case (proto::Parameterization::YAW_ONLY): - return make_unique>(); - case (proto::Parameterization::YAW_CONSTANT): - return make_unique>(); - case (proto::Parameterization::FIX_Z): - return make_unique( - 3, /* constant parameters */ std::vector{2}); - default: - LOG(FATAL) << "Parameterization is not known."; - } - return nullptr; -} - -} // namespace - -Parameterization::Parameterization(const proto::Parameterization& proto) - : proto_(proto), - ceres_parameterization_(CeresParameterizationFromProto(proto_)) {} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/parameterization/parameterization.h b/cartographer/pose_graph/node/parameterization/parameterization.h deleted file mode 100644 index eab1e58cc6..0000000000 --- a/cartographer/pose_graph/node/parameterization/parameterization.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ - -#include "cartographer/pose_graph/proto/node.pb.h" -#include "ceres/local_parameterization.h" - -namespace cartographer { -namespace pose_graph { - -class Parameterization { - public: - explicit Parameterization(const proto::Parameterization& proto); - - const proto::Parameterization& ToProto() const { return proto_; } - - ceres::LocalParameterization* ceres_parameterization() const { - return ceres_parameterization_.get(); - } - - private: - const proto::Parameterization proto_; - const std::unique_ptr ceres_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ diff --git a/cartographer/pose_graph/node/pose_2d.cc b/cartographer/pose_graph/node/pose_2d.cc deleted file mode 100644 index 50cba73eef..0000000000 --- a/cartographer/pose_graph/node/pose_2d.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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/node/pose_2d.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -constexpr size_t kXIndex = 0; -constexpr size_t kYIndex = 1; -constexpr size_t kRotationIndex = 2; - -} // namespace - -Pose2D::Pose2D(const NodeId& node_id, bool constant, - const proto::Pose2D& pose_2d) - : Node(node_id, constant), - pose_2d_{{pose_2d.translation().x(), pose_2d.translation().y(), - pose_2d.rotation()}} {} - -proto::Parameters Pose2D::ToParametersProto() const { - proto::Parameters parameters; - auto* pose_2d = parameters.mutable_pose_2d(); - pose_2d->set_rotation(pose_2d_[kRotationIndex]); - - auto* translation = pose_2d->mutable_translation(); - translation->set_x(pose_2d_[kXIndex]); - translation->set_y(pose_2d_[kYIndex]); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_2d.h b/cartographer/pose_graph/node/pose_2d.h deleted file mode 100644 index 5b94bb3e58..0000000000 --- a/cartographer/pose_graph/node/pose_2d.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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_NODE_POSE_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "Eigen/Core" - -namespace cartographer { -namespace pose_graph { - -class Pose2D : public Node { - public: - Pose2D(const NodeId& node_id, bool constant, const proto::Pose2D& pose_2d); - - std::array* mutable_pose_2d() { return &pose_2d_; } - const std::array& pose_2d() const { return pose_2d_; } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - std::array pose_2d_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ diff --git a/cartographer/pose_graph/node/pose_2d_test.cc b/cartographer/pose_graph/node/pose_2d_test.cc deleted file mode 100644 index 793277f3cc..0000000000 --- a/cartographer/pose_graph/node/pose_2d_test.cc +++ /dev/null @@ -1,49 +0,0 @@ -/* - * 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/node/pose_2d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "flat_world" timestamp: 1 } - constant: true - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -TEST(Pose2DTest, ToProto) { - Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true, - ParseProto(R"( - translation { x: 1 y: 2 } - rotation: 5 - )")); - EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_3d.cc b/cartographer/pose_graph/node/pose_3d.cc deleted file mode 100644 index d743e72ec8..0000000000 --- a/cartographer/pose_graph/node/pose_3d.cc +++ /dev/null @@ -1,54 +0,0 @@ -/* - * 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/node/pose_3d.h" - -namespace cartographer { -namespace pose_graph { - -Pose3D::Pose3D(const NodeId& node_id, bool constant, - const proto::Pose3D& pose_3d) - : Node(node_id, constant), - translation_{{pose_3d.translation().x(), pose_3d.translation().y(), - pose_3d.translation().z()}}, - translation_parameterization_(pose_3d.translation_parameterization()), - rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(), - pose_3d.rotation().z(), pose_3d.rotation().w()}}, - rotation_parameterization_(pose_3d.rotation_parameterization()) {} - -proto::Parameters Pose3D::ToParametersProto() const { - proto::Parameters parameters; - auto* pose_3d = parameters.mutable_pose_3d(); - - auto* translation = pose_3d->mutable_translation(); - translation->set_x(translation_[0]); - translation->set_y(translation_[1]); - translation->set_z(translation_[2]); - pose_3d->set_translation_parameterization( - translation_parameterization_.ToProto()); - - auto* rotation = pose_3d->mutable_rotation(); - rotation->set_x(rotation_[0]); - rotation->set_y(rotation_[1]); - rotation->set_z(rotation_[2]); - rotation->set_w(rotation_[3]); - pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto()); - - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_3d.h b/cartographer/pose_graph/node/pose_3d.h deleted file mode 100644 index 1b09781bc8..0000000000 --- a/cartographer/pose_graph/node/pose_3d.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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_NODE_POSE_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "cartographer/pose_graph/node/parameterization/parameterization.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -class Pose3D : public Node { - public: - Pose3D(const NodeId& node_id, bool constant, const proto::Pose3D& pose_3d); - - std::array* mutable_translation() { return &translation_; } - const std::array& translation() const { return translation_; } - ceres::LocalParameterization* translation_parameterization() const { - return translation_parameterization_.ceres_parameterization(); - } - - std::array* mutable_rotation() { return &rotation_; } - const std::array& rotation() const { return rotation_; } - ceres::LocalParameterization* rotation_parameterization() const { - return rotation_parameterization_.ceres_parameterization(); - } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - std::array translation_; - Parameterization translation_parameterization_; - - std::array rotation_; - Parameterization rotation_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ diff --git a/cartographer/pose_graph/node/pose_3d_test.cc b/cartographer/pose_graph/node/pose_3d_test.cc deleted file mode 100644 index 25b1059a5d..0000000000 --- a/cartographer/pose_graph/node/pose_3d_test.cc +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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/node/pose_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "bumpy_world" timestamp: 1 } - constant: true - parameters { - pose_3d { - translation { x: 1 y: 2 z: 3 } - translation_parameterization: FIX_Z - rotation: { w: 0 x: 1 y: 2 z: 3 } - } - } -)PROTO"; - -TEST(Pose3DTest, ToProto) { - Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true, - ParseProto(R"( - translation { x: 1 y: 2 z: 3 } - translation_parameterization: FIX_Z - rotation: { w: 0 x: 1 y: 2 z: 3 } - rotation_parameterization: NONE - )")); - EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_controller.cc b/cartographer/pose_graph/pose_graph_controller.cc deleted file mode 100644 index 5a7aa2cec3..0000000000 --- a/cartographer/pose_graph/pose_graph_controller.cc +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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/pose_graph_controller.h" - -namespace cartographer { -namespace pose_graph { - -void PoseGraphController::AddData(const proto::PoseGraphData& data) { - absl::MutexLock locker(&mutex_); - for (const auto& node : data.nodes()) { - AddNodeToPoseGraphData(node, &data_); - } - for (const auto& constraint : data.constraints()) { - AddConstraintToPoseGraphData(constraint, &data_); - } -} - -void PoseGraphController::AddNode(const proto::Node& node) { - absl::MutexLock locker(&mutex_); - AddNodeToPoseGraphData(node, &data_); -} - -void PoseGraphController::AddConstraint(const proto::Constraint& constraint) { - absl::MutexLock locker(&mutex_); - AddConstraintToPoseGraphData(constraint, &data_); -} - -Solver::SolverStatus PoseGraphController::Optimize() { - absl::MutexLock locker(&mutex_); - return solver_->Solve(&data_); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_controller.h b/cartographer/pose_graph/pose_graph_controller.h deleted file mode 100644 index 3c915edfc6..0000000000 --- a/cartographer/pose_graph/pose_graph_controller.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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_POSE_GRAPH_CONTROLLER_H_ -#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ - -#include "absl/synchronization/mutex.h" -#include "cartographer/pose_graph/pose_graph_data.h" -#include "cartographer/pose_graph/proto/pose_graph_data.pb.h" -#include "cartographer/pose_graph/solver/solver.h" - -namespace cartographer { -namespace pose_graph { - -class PoseGraphController { - public: - PoseGraphController(std::unique_ptr optimizer) - : solver_(std::move(optimizer)) {} - - PoseGraphController(const PoseGraphController&) = delete; - PoseGraphController& operator=(const PoseGraphController&) = delete; - - void AddData(const proto::PoseGraphData& data) LOCKS_EXCLUDED(mutex_); - void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_); - void AddConstraint(const proto::Constraint& constraint) - LOCKS_EXCLUDED(mutex_); - - Solver::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); - - private: - std::unique_ptr solver_; - - mutable absl::Mutex mutex_; - PoseGraphData data_ GUARDED_BY(mutex_); -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ diff --git a/cartographer/pose_graph/pose_graph_data.cc b/cartographer/pose_graph/pose_graph_data.cc deleted file mode 100644 index 87686c9252..0000000000 --- a/cartographer/pose_graph/pose_graph_data.cc +++ /dev/null @@ -1,100 +0,0 @@ -/* - * 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/pose_graph_data.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h" -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h" -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h" -#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using absl::make_unique; - -std::unique_ptr CreateConstraint( - const proto::Constraint& constraint) { - const auto& id = constraint.id(); - const auto& loss = constraint.loss_function(); - const auto& cost = constraint.cost_function(); - switch (cost.type_case()) { - case (proto::CostFunction::kRelativePose2D): - return make_unique(id, loss, - cost.relative_pose_2d()); - case (proto::CostFunction::kRelativePose3D): - return make_unique(id, loss, - cost.relative_pose_3d()); - case (proto::CostFunction::kAcceleration3D): - return make_unique(id, loss, - cost.acceleration_3d()); - case (proto::CostFunction::kRotation3D): - return make_unique(id, loss, cost.rotation_3d()); - case (proto::CostFunction::kInterpolatedRelativePose2D): - return make_unique( - id, loss, cost.interpolated_relative_pose_2d()); - case (proto::CostFunction::kInterpolatedRelativePose3D): - return make_unique( - id, loss, cost.interpolated_relative_pose_3d()); - case (proto::CostFunction::TYPE_NOT_SET): - LOG(FATAL) << "Constraint cost function type is not set."; - } - return nullptr; -} - -} // namespace - -void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) { - NodeId node_id(node.id()); - switch (node.parameters().type_case()) { - case (proto::Parameters::kPose2D): { - data->nodes.pose_2d_nodes.emplace( - node_id, make_unique(node_id, node.constant(), - node.parameters().pose_2d())); - return; - } - case (proto::Parameters::kPose3D): { - data->nodes.pose_3d_nodes.emplace( - node_id, make_unique(node_id, node.constant(), - node.parameters().pose_3d())); - - return; - } - case (proto::Parameters::kImuCalibration): { - data->nodes.imu_calibration_nodes.emplace( - node_id, - make_unique(node_id, node.constant(), - node.parameters().imu_calibration())); - return; - } - case (proto::Parameters::TYPE_NOT_SET): { - LOG(FATAL) << "Node parameter type is not set."; - return; - } - } -} - -void AddConstraintToPoseGraphData(const proto::Constraint& constraint, - PoseGraphData* data) { - data->constraints.emplace_back(CreateConstraint(constraint)); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_data.h b/cartographer/pose_graph/pose_graph_data.h deleted file mode 100644 index 1a6b30c84c..0000000000 --- a/cartographer/pose_graph/pose_graph_data.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * 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_POSE_GRAPH_DATA_H_ -#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/node/nodes.h" - -namespace cartographer { -namespace pose_graph { - -struct PoseGraphData { - Nodes nodes; - std::vector> constraints; -}; - -void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data); -void AddConstraintToPoseGraphData(const proto::Constraint& constraint, - PoseGraphData* data); - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ diff --git a/cartographer/pose_graph/proto/constraint.proto b/cartographer/pose_graph/proto/constraint.proto deleted file mode 100644 index 44b6d0538d..0000000000 --- a/cartographer/pose_graph/proto/constraint.proto +++ /dev/null @@ -1,26 +0,0 @@ -// 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. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/loss_function.proto"; -import "cartographer/pose_graph/proto/cost_function.proto"; - -package cartographer.pose_graph.proto; - -message Constraint { - string id = 1; - CostFunction cost_function = 2; - LossFunction loss_function = 3; -} diff --git a/cartographer/pose_graph/proto/cost_function.proto b/cartographer/pose_graph/proto/cost_function.proto deleted file mode 100644 index 877f66f4c7..0000000000 --- a/cartographer/pose_graph/proto/cost_function.proto +++ /dev/null @@ -1,114 +0,0 @@ -// 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. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/node.proto"; -import "cartographer/transform/proto/transform.proto"; - -package cartographer.pose_graph.proto; - -message RelativePose2D { - NodeId first = 1; - NodeId second = 2; - - message Parameters { - transform.proto.Rigid2d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - } - Parameters parameters = 3; -} - -message RelativePose3D { - NodeId first = 1; - NodeId second = 2; - - message Parameters { - transform.proto.Rigid3d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - } - Parameters parameters = 3; -} - -message Acceleration3D { - NodeId first = 1; - NodeId second = 2; - NodeId third = 3; - NodeId imu_calibration = 4; - - message Parameters { - transform.proto.Vector3d delta_velocity_imu_frame = 1; - double first_to_second_delta_time_seconds = 2; - double second_to_third_delta_time_seconds = 3; - double scaling_factor = 4; - } - Parameters parameters = 5; -} - -message Rotation3D { - NodeId first = 1; - NodeId second = 2; - NodeId imu_calibration = 3; - - message Parameters { - transform.proto.Quaterniond delta_rotation_imu_frame = 1; - double scaling_factor = 2; - } - Parameters parameters = 4; -} - -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 = 4; -} - -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; - // Interpolates between first_start and first_end. - double interpolation_factor = 4; - } - Parameters parameters = 4; -} - -message CostFunction { - oneof type { - RelativePose2D relative_pose_2d = 1; - RelativePose3D relative_pose_3d = 2; - Acceleration3D acceleration_3d = 3; - Rotation3D rotation_3d = 4; - InterpolatedRelativePose2D interpolated_relative_pose_2d = 5; - InterpolatedRelativePose3D interpolated_relative_pose_3d = 6; - } -} diff --git a/cartographer/pose_graph/proto/loss_function.proto b/cartographer/pose_graph/proto/loss_function.proto deleted file mode 100644 index a45e557f97..0000000000 --- a/cartographer/pose_graph/proto/loss_function.proto +++ /dev/null @@ -1,30 +0,0 @@ -// 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. - -syntax = "proto3"; - -package cartographer.pose_graph.proto; - -message QuadraticLoss {} - -message HuberLoss { - double scale = 1; -} - -message LossFunction { - oneof Type { - QuadraticLoss quadratic_loss = 1; - HuberLoss huber_loss = 2; - } -} diff --git a/cartographer/pose_graph/proto/node.proto b/cartographer/pose_graph/proto/node.proto deleted file mode 100644 index 597a95ea8c..0000000000 --- a/cartographer/pose_graph/proto/node.proto +++ /dev/null @@ -1,65 +0,0 @@ -// 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. - -syntax = "proto3"; - -package cartographer.pose_graph.proto; - -import "cartographer/transform/proto/transform.proto"; - -enum Parameterization { - NONE = 0; - FIX_Z = 1; - YAW_ONLY = 2; - YAW_CONSTANT = 3; -} - -message Pose2D { - transform.proto.Vector2d translation = 1; - double rotation = 2; -} - -message Pose3D { - transform.proto.Vector3d translation = 1; - transform.proto.Quaterniond rotation = 2; - Parameterization translation_parameterization = 3; - Parameterization rotation_parameterization = 4; -} - -message ImuCalibration { - double gravity_constant = 1; - transform.proto.Quaterniond orientation = 2; - Parameterization orientation_parameterization = 3; -} - -message NodeId { - string object_id = 1; - string group_id = 2; - int64 timestamp = 3; -} - -message Parameters { - oneof type { - Pose2D pose_2d = 1; - Pose3D pose_3d = 2; - ImuCalibration imu_calibration = 3; - } -} - -message Node { - NodeId id = 1; - bool constant = 2; - Parameters parameters = 3; -} - diff --git a/cartographer/pose_graph/proto/pose_graph_data.proto b/cartographer/pose_graph/proto/pose_graph_data.proto deleted file mode 100644 index b76966ae85..0000000000 --- a/cartographer/pose_graph/proto/pose_graph_data.proto +++ /dev/null @@ -1,25 +0,0 @@ -// 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. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/node.proto"; -import "cartographer/pose_graph/proto/constraint.proto"; - -package cartographer.pose_graph.proto; - -message PoseGraphData { - repeated Node nodes = 1; - repeated Constraint constraints = 2; -} diff --git a/cartographer/pose_graph/proto/solver_config.proto b/cartographer/pose_graph/proto/solver_config.proto deleted file mode 100644 index cec67ccdb9..0000000000 --- a/cartographer/pose_graph/proto/solver_config.proto +++ /dev/null @@ -1,14 +0,0 @@ -syntax = "proto3"; - -import "cartographer/common/proto/ceres_solver_options.proto"; - -package cartographer.pose_graph.proto; - -// Configuration for the non-linear least squares solver. -message SolverConfig { - // If true, logs a summary of the optimization in every iteration. - bool log_solver_summary = 1; - - // Options specific for the Ceres solver. - cartographer.common.proto.CeresSolverOptions ceres_options = 2; -} diff --git a/cartographer/pose_graph/solver/ceres_solver.cc b/cartographer/pose_graph/solver/ceres_solver.cc deleted file mode 100644 index 3a7421b3db..0000000000 --- a/cartographer/pose_graph/solver/ceres_solver.cc +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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/solver/ceres_solver.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -ceres::Problem::Options CreateCeresProblemOptions() { - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - return problem_options; -} - -Solver::SolverStatus ToSolverStatus( - const ceres::TerminationType& termination_type) { - switch (termination_type) { - case (ceres::TerminationType::CONVERGENCE): - return Solver::SolverStatus::CONVERGENCE; - case (ceres::TerminationType::NO_CONVERGENCE): - return Solver::SolverStatus::NO_CONVERGENCE; - default: - return Solver::SolverStatus::FAILURE; - } -} - -} // namespace - -CeresSolver::CeresSolver(const ceres::Solver::Options& options) - : problem_options_(CreateCeresProblemOptions()), solver_options_(options) {} - -Solver::SolverStatus CeresSolver::Solve(PoseGraphData* data) const { - ceres::Problem problem(problem_options_); - - for (const auto& constraint : data->constraints) { - constraint->AddToSolver(&data->nodes, &problem); - } - - ceres::Solver::Summary summary; - ceres::Solve(solver_options_, &problem, &summary); - return ToSolverStatus(summary.termination_type); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/solver/ceres_solver.h b/cartographer/pose_graph/solver/ceres_solver.h deleted file mode 100644 index fcaa36544e..0000000000 --- a/cartographer/pose_graph/solver/ceres_solver.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * 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_OPTIMIZER_CERES_SOLVER_H_ -#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ - -#include "cartographer/pose_graph/solver/solver.h" -#include "ceres/problem.h" -#include "ceres/solver.h" - -namespace cartographer { -namespace pose_graph { - -class CeresSolver : public Solver { - public: - explicit CeresSolver(const ceres::Solver::Options& options); - - SolverStatus Solve(PoseGraphData* data) const final; - - private: - const ceres::Problem::Options problem_options_; - const ceres::Solver::Options solver_options_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ diff --git a/cartographer/pose_graph/solver/ceres_solver_test.cc b/cartographer/pose_graph/solver/ceres_solver_test.cc deleted file mode 100644 index f86b9ae5b1..0000000000 --- a/cartographer/pose_graph/solver/ceres_solver_test.cc +++ /dev/null @@ -1,82 +0,0 @@ -/* - * 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/solver/ceres_solver.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kStartNode[] = R"PROTO( - id { object_id: "start_node" timestamp: 1 } - constant: false - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -constexpr char kEndNode[] = R"PROTO( - id { object_id: "end_node" timestamp: 1 } - constant: false - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -constexpr char kRelativePose2D[] = R"PROTO( - id: "constraint_1" - loss_function { quadratic_loss: {} } - cost_function { - relative_pose_2d { - first { object_id: "start_node" timestamp: 1 } - second { object_id: "end_node" timestamp: 1 } - parameters { - first_t_second { - translation { x: 1 y: 1 } - rotation: 0 - } - translation_weight: 1 - rotation_weight: 1 - } - } - } -)PROTO"; - -TEST(CeresSolverTest, SmokeTest) { - PoseGraphData data; - AddNodeToPoseGraphData(ParseProto(kStartNode), &data); - AddNodeToPoseGraphData(ParseProto(kEndNode), &data); - AddConstraintToPoseGraphData(ParseProto(kRelativePose2D), - &data); - CeresSolver optimizer(ceres::Solver::Options{}); - EXPECT_EQ(optimizer.Solve(&data), Solver::SolverStatus::CONVERGENCE); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/solver/solver.h b/cartographer/pose_graph/solver/solver.h deleted file mode 100644 index 11bb2a199d..0000000000 --- a/cartographer/pose_graph/solver/solver.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * 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_SOLVER_SOLVER_H_ -#define CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ - -#include "cartographer/pose_graph/pose_graph_data.h" - -namespace cartographer { -namespace pose_graph { - -class Solver { - public: - enum class SolverStatus { - CONVERGENCE, - NO_CONVERGENCE, - FAILURE, - }; - - Solver() = default; - virtual ~Solver() = default; - - Solver(const Solver&) = delete; - Solver& operator=(const Solver&) = delete; - - virtual SolverStatus Solve(PoseGraphData* data) const = 0; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ From a6050b5870c6279032ce1aba090b551bcbc9ccc9 Mon Sep 17 00:00:00 2001 From: Andre Gaschler Date: Wed, 20 Mar 2019 09:30:27 +0100 Subject: [PATCH 02/99] Update README.rst (#1542) --- README.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.rst b/README.rst index b482e2a69e..9d66627a65 100644 --- a/README.rst +++ b/README.rst @@ -45,11 +45,12 @@ Open house We regularly meet in an open-for-all Google hangout to discuss progress and plans for Cartographer. You can join the `mailing list`_ to receive announcements. -The next Cartographer Open House Hangout is on **Thursday, March 14th 2019, 5pm CET (9am PDT)** [`Hangouts link`_]. +The next Cartographer Open House Hangout is on **Thursday, April 11th 2019, 5pm CEST (8am PDT)** [`Hangouts link`_]. .. _mailing list: https://groups.google.com/forum/#!forum/google-cartographer .. _Hangouts link: https://staging.talkgadget.google.com/hangouts/_/google.com/cartographeropenhouse +- March 14, 2019: `Slides `_ - February 21, 2019: `Slides `_ - January 17, 2019: `Slides `_ - November 22, 2018: `Slides `_ From 111d7bb5aac7c8ba6d5d3125de2d2fe4ddcec2e4 Mon Sep 17 00:00:00 2001 From: Marco Feuerstein Date: Tue, 26 Mar 2019 11:33:03 +0100 Subject: [PATCH 03/99] Ability to pass additional comments to ply header. (#1549) --- .../io/ply_writing_points_processor.cc | 21 +++++++++++++------ .../io/ply_writing_points_processor.h | 2 ++ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index 6c13d23c47..8b0ecdbafb 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -33,6 +33,7 @@ namespace { // Writes the PLY header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities, + const std::vector& comments, const int64 num_points, FileWriter* const file_writer) { const std::string color_header = !has_color ? "" @@ -44,8 +45,11 @@ void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities, std::ostringstream stream; stream << "ply\n" << "format binary_little_endian 1.0\n" - << "comment generated by Cartographer\n" - << "element vertex " << std::setw(15) << std::setfill('0') + << "comment generated by Cartographer\n"; + for (const std::string& comment : comments) { + stream << "comment " << comment << "\n"; + } + stream << "element vertex " << std::setw(15) << std::setfill('0') << num_points << "\n" << "property float x\n" << "property float y\n" @@ -86,18 +90,22 @@ PlyWritingPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return absl::make_unique( - file_writer_factory(dictionary->GetString("filename")), next); + file_writer_factory(dictionary->GetString("filename")), + std::vector(), next); } PlyWritingPointsProcessor::PlyWritingPointsProcessor( - std::unique_ptr file_writer, PointsProcessor* const next) + std::unique_ptr file_writer, + const std::vector& comments, PointsProcessor* const next) : next_(next), + comments_(comments), num_points_(0), has_colors_(false), file_(std::move(file_writer)) {} PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { - WriteBinaryPlyHeader(has_colors_, has_intensities_, num_points_, file_.get()); + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, num_points_, + file_.get()); CHECK(file_->Close()) << "Closing PLY file_writer failed."; switch (next_->Flush()) { @@ -120,7 +128,8 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); has_intensities_ = !batch->intensities.empty(); - WriteBinaryPlyHeader(has_colors_, has_intensities_, 0, file_.get()); + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, 0, + file_.get()); } if (has_colors_) { CHECK_EQ(batch->points.size(), batch->colors.size()) diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h index 8f07de9276..76295870b3 100644 --- a/cartographer/io/ply_writing_points_processor.h +++ b/cartographer/io/ply_writing_points_processor.h @@ -26,6 +26,7 @@ class PlyWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_ply"; PlyWritingPointsProcessor(std::unique_ptr file_writer, + const std::vector& comments, PointsProcessor* next); static std::unique_ptr FromDictionary( @@ -44,6 +45,7 @@ class PlyWritingPointsProcessor : public PointsProcessor { private: PointsProcessor* const next_; + std::vector comments_; int64 num_points_; bool has_colors_; bool has_intensities_; From b1edbea84f1ce23d64ee34ef32a74ebfdba9910a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Sat, 30 Mar 2019 22:13:12 +0100 Subject: [PATCH 04/99] Update bazel dependencies (#1553) * Update dependencies Update protobuf grpc and bazel-rules and added bazel-skylib (required by protobuf 3.7.1). Closes #1519 --- bazel/repositories.bzl | 31 +++++++++++++++++++------------ scripts/install_debs_cmake.sh | 3 ++- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index 8e66a81a54..15135941a3 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -31,11 +31,10 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_antonovvk_bazel_rules", - sha256 = "ba75b07d3fd297375a6688e9a16583eb616e7a74b3d5e8791e7a222cf36ab26e", - strip_prefix = "bazel_rules-98ddd7e4f7c63ea0868f08bcc228463dac2f9f12", + sha256 = "2f5327a2dc9a0cc8ead93953a5d2ae2e0308aece685e46cc89c27538a7e9a73a", + strip_prefix = "bazel_rules-c76e47ebe6f0a03b9dd99e245d5a0611813c36f9", urls = [ - "https://mirror.bazel.build/github.com/antonovvk/bazel_rules/archive/98ddd7e4f7c63ea0868f08bcc228463dac2f9f12.tar.gz", - "https://github.com/antonovvk/bazel_rules/archive/98ddd7e4f7c63ea0868f08bcc228463dac2f9f12.tar.gz", + "https://github.com/drigz/bazel_rules/archive/c76e47ebe6f0a03b9dd99e245d5a0611813c36f9.tar.gz", ], ) @@ -204,14 +203,22 @@ def cartographer_repositories(): ], ) + _maybe( + http_archive, + name = "bazel_skylib", + sha256 = "bbccf674aa441c266df9894182d80de104cabd19be98be002f6d478aaa31574d", + strip_prefix = "bazel-skylib-2169ae1c374aab4a09aa90e65efe1a3aad4e279b", + urls = ["https://github.com/bazelbuild/bazel-skylib/archive/2169ae1c374aab4a09aa90e65efe1a3aad4e279b.tar.gz"], + ) + _maybe( http_archive, name = "com_google_protobuf", - sha256 = "2244b0308846bb22b4ff0bcc675e99290ff9f1115553ae9671eba1030af31bc0", - strip_prefix = "protobuf-3.6.1.2", + sha256 = "f1748989842b46fa208b2a6e4e2785133cfcc3e4d43c17fecb023733f0f5443f", + strip_prefix = "protobuf-3.7.1", urls = [ - "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.6.1.2.tar.gz", - "https://github.com/google/protobuf/archive/v3.6.1.2.tar.gz", + "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.7.1.tar.gz", + "https://github.com/google/protobuf/archive/v3.7.1.tar.gz", ], ) @@ -230,11 +237,11 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_grpc_grpc", - sha256 = "e699efa9422e071a42f052ba8369fbc810e6f7c6fb0a5b1c021f54ac1a92a1f3", - strip_prefix = "grpc-b250f34b1225cde1bb19496c5cc5d66e40111052", + sha256 = "f869c648090e8bddaa1260a271b1089caccbe735bf47ac9cd7d44d35a02fb129", + strip_prefix = "grpc-1.19.1", urls = [ - "https://mirror.bazel.build/github.com/grpc/grpc/archive/b250f34b1225cde1bb19496c5cc5d66e40111052.tar.gz", - "https://github.com/grpc/grpc/archive/b250f34b1225cde1bb19496c5cc5d66e40111052.tar.gz", + "https://mirror.bazel.build/github.com/grpc/grpc/archive/v1.19.1.tar.gz", + "https://github.com/grpc/grpc/archive/v1.19.1.tar.gz", ], ) diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index bfa67a7277..6a01bcb11a 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -27,7 +27,8 @@ then sudo apt-get install cmake3 -y elif [[ "$(lsb_release -sc)" = "jessie" ]] then - sudo sh -c "echo 'deb http://ftp.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" + sudo sh -c "echo 'deb [check-valid-until=no] http://archive.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" + sudo sh -c "echo 'Acquire::Check-Valid-Until \"false\";' >> /etc/apt/apt.conf" sudo apt-get update sudo apt-get -t jessie-backports install cmake -y else From 98197cff4f0fd68bc1ee66d7ed64b9dc7be4f701 Mon Sep 17 00:00:00 2001 From: Andre Gaschler Date: Tue, 2 Apr 2019 09:35:20 +0200 Subject: [PATCH 05/99] Fix Bazel test failure (#1557) FIXES=#1519 --- cartographer/cloud/BUILD.bazel | 2 ++ cartographer/cloud/internal/local_trajectory_uploader_test.cc | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/cartographer/cloud/BUILD.bazel b/cartographer/cloud/BUILD.bazel index c38f0a901f..57d8f7b6a2 100644 --- a/cartographer/cloud/BUILD.bazel +++ b/cartographer/cloud/BUILD.bazel @@ -91,6 +91,8 @@ cc_binary( name = src.replace("/", "_").replace(".cc", ""), srcs = [src], data = ["//:configuration_files"], + # TODO(gaschler): Fix UplinkServerRestarting test for Bazel. + args = ["--gtest_filter=-ClientServerTestByGridType/ClientServerTestByGridType.LocalSlam2DUplinkServerRestarting*"], flaky = True, # :internal_client_server_test sometimes fails. # Tests cannot run concurrently as some of them open the same port. tags = ["exclusive"], diff --git a/cartographer/cloud/internal/local_trajectory_uploader_test.cc b/cartographer/cloud/internal/local_trajectory_uploader_test.cc index 83b8abccbd..b333dc9fc5 100644 --- a/cartographer/cloud/internal/local_trajectory_uploader_test.cc +++ b/cartographer/cloud/internal/local_trajectory_uploader_test.cc @@ -33,7 +33,7 @@ const int kLocalTrajectoryId = 3; TEST(LocalTrajectoryUploaderTest, HandlesInvalidUplink) { auto uploader = CreateLocalTrajectoryUploader("invalid-uplink-address:50051", - /*batch_size=*/1, false, ""); + /*batch_size=*/1, false, false); uploader->Start(); mapping::proto::TrajectoryBuilderOptions options; auto status = uploader->AddTrajectory( From ab0ceacbed8c42a47bf6cf26dfd083a74d09f20b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Apr 2019 16:01:51 +0200 Subject: [PATCH 06/99] Check for empty basename in GetFileContentOrDie() (#1558) An empty basename would mean a directory path is returned by GetFullPathOrDie(), which then leads to a cryptic exception when trying to read from it using the istreambuf_iterator: "basic_filebuf::underflow error reading the file: iostream error" --- cartographer/common/configuration_file_resolver.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/cartographer/common/configuration_file_resolver.cc b/cartographer/common/configuration_file_resolver.cc index 9774caf104..3c2fd10c0e 100644 --- a/cartographer/common/configuration_file_resolver.cc +++ b/cartographer/common/configuration_file_resolver.cc @@ -47,6 +47,7 @@ std::string ConfigurationFileResolver::GetFullPathOrDie( std::string ConfigurationFileResolver::GetFileContentOrDie( const std::string& basename) { + CHECK(!basename.empty()) << "File basename cannot be empty." << basename; const std::string filename = GetFullPathOrDie(basename); std::ifstream stream(filename.c_str()); return std::string((std::istreambuf_iterator(stream)), From dc745a147a02548c21928c93423ae44f3e863d9b Mon Sep 17 00:00:00 2001 From: jie Date: Tue, 2 Apr 2019 07:57:27 -0700 Subject: [PATCH 07/99] Add saturation_factor to scale the saturation of the point color (#1556) Add saturation_factor to scale the saturation of the point color to get darker XRay for sparse point cloud. --- cartographer/io/xray_points_processor.cc | 19 +++++++++++++------ cartographer/io/xray_points_processor.h | 7 ++++++- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 4612e6692b..282bdda9b8 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -67,7 +67,7 @@ float Mix(const float a, const float b, const float t) { } // Convert 'matrix' into a pleasing-to-look-at image. -Image IntoImage(const PixelDataMatrix& matrix) { +Image IntoImage(const PixelDataMatrix& matrix, double saturation_factor) { Image image(matrix.width(), matrix.height()); float max = std::numeric_limits::min(); for (int y = 0; y < matrix.height(); ++y) { @@ -92,7 +92,8 @@ Image IntoImage(const PixelDataMatrix& matrix) { // basic idea here was that walls (full height) are fully saturated, but // details like chairs and tables are still well visible. const float saturation = - std::log(cell.num_occupied_cells_in_column) / max; + std::min(1.0, std::log(cell.num_occupied_cells_in_column) / + max * saturation_factor); const FloatColor color = {{Mix(1.f, cell.mean_r, saturation), Mix(1.f, cell.mean_g, saturation), Mix(1.f, cell.mean_b, saturation)}}; @@ -115,7 +116,8 @@ bool ContainedIn(const common::Time& time, } // namespace XRayPointsProcessor::XRayPointsProcessor( - const double voxel_size, const transform::Rigid3f& transform, + const double voxel_size, const double saturation_factor, + const transform::Rigid3f& transform, const std::vector& floors, const DrawTrajectories& draw_trajectories, const std::string& output_filename, @@ -127,7 +129,8 @@ XRayPointsProcessor::XRayPointsProcessor( next_(next), floors_(floors), output_filename_(output_filename), - transform_(transform) { + transform_(transform), + saturation_factor_(saturation_factor) { for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { aggregations_.emplace_back( Aggregation{mapping::HybridGridBase(voxel_size), {}}); @@ -146,6 +149,10 @@ std::unique_ptr XRayPointsProcessor::FromDictionary( dictionary->GetBool("draw_trajectories")) ? DrawTrajectories::kYes : DrawTrajectories::kNo; + const double saturation_factor = + dictionary->HasKey("saturation_factor") + ? dictionary->GetDouble("saturation_factor") + : 1.; if (separate_floor) { CHECK_EQ(trajectories.size(), 1) << "Can only detect floors with a single trajectory."; @@ -153,7 +160,7 @@ std::unique_ptr XRayPointsProcessor::FromDictionary( } return absl::make_unique( - dictionary->GetDouble("voxel_size"), + dictionary->GetDouble("voxel_size"), saturation_factor, transform::FromDictionary(dictionary->GetDictionary("transform").get()) .cast(), floors, draw_trajectories, dictionary->GetString("filename"), @@ -193,7 +200,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, ++pixel_data.num_occupied_cells_in_column; } - Image image = IntoImage(pixel_data_matrix); + Image image = IntoImage(pixel_data_matrix, saturation_factor_); if (draw_trajectories_ == DrawTrajectories::kYes) { for (size_t i = 0; i < trajectories_.size(); ++i) { DrawTrajectory( diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 1d269a89a7..8d0f7585b4 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -38,7 +38,8 @@ class XRayPointsProcessor : public PointsProcessor { "write_xray_image"; enum class DrawTrajectories { kNo, kYes }; XRayPointsProcessor( - double voxel_size, const transform::Rigid3f& transform, + double voxel_size, double saturation_factor, + const transform::Rigid3f& transform, const std::vector& floors, const DrawTrajectories& draw_trajectories, const std::string& output_filename, @@ -90,6 +91,10 @@ class XRayPointsProcessor : public PointsProcessor { // Bounding box containing all cells with data in all 'aggregations_'. Eigen::AlignedBox3i bounding_box_; + + // Scale the saturation of the point color. If saturation_factor_ > 1, the + // point has darker color, otherwise it has lighter color. + const double saturation_factor_; }; } // namespace io From 035162d86546c63ad431521c1c2fec9d0fe24c37 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 10 Apr 2019 13:04:57 +0200 Subject: [PATCH 08/99] Update clang-format to new Google style. (#1564) Apparently the format bot uses a bleeding edge clang-format that uses the new Google style and reformats a bunch of files in every PR. This is an empty commit to trigger this in a separate commit. See https://github.com/llvm-mirror/clang/commit/62e3198c4f5490a1c60ba51d81fe2e1f0dc99135 --- cartographer/cloud/internal/client/pose_graph_stub.cc | 1 + .../handlers/add_fixed_frame_pose_data_handler_test.cc | 1 + .../cloud/internal/handlers/add_imu_data_handler_test.cc | 1 + .../internal/handlers/add_landmark_data_handler_test.cc | 1 + .../internal/handlers/add_odometry_data_handler_test.cc | 1 + .../internal/handlers/add_rangefinder_data_handler_test.cc | 1 + .../cloud/internal/handlers/add_trajectory_handler_test.cc | 1 + .../internal/handlers/get_landmark_poses_handler_test.cc | 1 + .../internal/handlers/set_landmark_pose_handler_test.cc | 1 + cartographer/cloud/internal/map_builder_context_impl.cc | 2 -- cartographer/common/lockless_queue_test.cc | 1 + cartographer/common/port.h | 7 +++---- cartographer/common/time.cc | 1 + cartographer/io/fake_file_writer_test.cc | 3 ++- cartographer/io/internal/in_memory_proto_stream_test.cc | 2 +- cartographer/io/internal/mapping_state_serialization.cc | 1 + cartographer/io/probability_grid_points_processor_test.cc | 1 + cartographer/io/proto_stream.cc | 1 + cartographer/io/proto_stream_deserializer_test.cc | 3 ++- cartographer/io/testing/test_helpers.cc | 1 + cartographer/mapping/2d/range_data_inserter_2d_test.cc | 3 +-- cartographer/mapping/2d/submap_2d_test.cc | 2 +- cartographer/mapping/detect_floors.h | 3 +-- cartographer/mapping/internal/2d/local_slam_result_2d.cc | 1 + cartographer/mapping/internal/2d/normal_estimation_2d.h | 1 + .../2d/scan_matching/correlative_scan_matcher_test.cc | 1 - .../2d/scan_matching/occupied_space_cost_function_2d.cc | 1 + .../scan_matching/occupied_space_cost_function_2d_test.cc | 1 - .../2d/scan_matching/tsdf_match_cost_function_2d_test.cc | 1 - cartographer/mapping/internal/3d/local_slam_result_3d.cc | 1 + .../internal/constraints/constraint_builder_2d_test.cc | 2 +- cartographer/mapping/map_builder.h | 3 +-- cartographer/mapping/range_data_inserter_interface.cc | 1 + cartographer/sensor/point_cloud_test.cc | 2 +- 34 files changed, 34 insertions(+), 21 deletions(-) diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 76f1c88d38..f7ff9ad14d 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/client/pose_graph_stub.h" + #include "async_grpc/client.h" #include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/get_all_submap_poses.h" diff --git a/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc index f92d2c7608..33c0dbf0cf 100644 --- a/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc index 679f206860..4052a86331 100644 --- a/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc index 0b72934692..5f772cd64a 100644 --- a/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc index 6187e8ac52..e0571a8b4f 100644 --- a/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc index a7af01e089..63b0642a3b 100644 --- a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc b/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc index d05ec5d600..4bbb209662 100644 --- a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" + #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" diff --git a/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc b/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc index 20067fae95..be45875afa 100644 --- a/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc +++ b/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc index 015b5171d1..485c594331 100644 --- a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc +++ b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h" diff --git a/cartographer/cloud/internal/map_builder_context_impl.cc b/cartographer/cloud/internal/map_builder_context_impl.cc index 4606046f12..3f3c46212c 100644 --- a/cartographer/cloud/internal/map_builder_context_impl.cc +++ b/cartographer/cloud/internal/map_builder_context_impl.cc @@ -14,8 +14,6 @@ * limitations under the License. */ -#include "cartographer/cloud/internal/map_builder_server.h" - #include "cartographer/cloud/internal/map_builder_server.h" #include "cartographer/mapping/internal/2d/local_slam_result_2d.h" #include "cartographer/mapping/internal/3d/local_slam_result_3d.h" diff --git a/cartographer/common/lockless_queue_test.cc b/cartographer/common/lockless_queue_test.cc index f1f247c4ff..5553c35ebf 100644 --- a/cartographer/common/lockless_queue_test.cc +++ b/cartographer/common/lockless_queue_test.cc @@ -1,4 +1,5 @@ #include "cartographer/common/lockless_queue.h" + #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/common/port.h b/cartographer/common/port.h index 338861f318..eec84697b8 100644 --- a/cartographer/common/port.h +++ b/cartographer/common/port.h @@ -17,13 +17,12 @@ #ifndef CARTOGRAPHER_COMMON_PORT_H_ #define CARTOGRAPHER_COMMON_PORT_H_ -#include -#include -#include - #include #include #include +#include +#include +#include namespace cartographer { diff --git a/cartographer/common/time.cc b/cartographer/common/time.cc index a2c90d227c..e74eead417 100644 --- a/cartographer/common/time.cc +++ b/cartographer/common/time.cc @@ -17,6 +17,7 @@ #include "cartographer/common/time.h" #include + #include #include #include diff --git a/cartographer/io/fake_file_writer_test.cc b/cartographer/io/fake_file_writer_test.cc index 6189b92135..711e9164c2 100644 --- a/cartographer/io/fake_file_writer_test.cc +++ b/cartographer/io/fake_file_writer_test.cc @@ -14,9 +14,10 @@ * limitations under the License. */ +#include "cartographer/io/fake_file_writer.h" + #include -#include "cartographer/io/fake_file_writer.h" #include "glog/logging.h" #include "gtest/gtest.h" diff --git a/cartographer/io/internal/in_memory_proto_stream_test.cc b/cartographer/io/internal/in_memory_proto_stream_test.cc index 9dd3a1e83c..e274472aaf 100644 --- a/cartographer/io/internal/in_memory_proto_stream_test.cc +++ b/cartographer/io/internal/in_memory_proto_stream_test.cc @@ -15,9 +15,9 @@ */ #include "cartographer/io/internal/in_memory_proto_stream.h" + #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" - #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index a34c8678a9..8305e7ade3 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -15,6 +15,7 @@ */ #include "cartographer/io/internal/mapping_state_serialization.h" + #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index b3fc73f2c2..99753bb5be 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -17,6 +17,7 @@ #include "cartographer/io/probability_grid_points_processor.h" #include + #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" diff --git a/cartographer/io/proto_stream.cc b/cartographer/io/proto_stream.cc index f55a3a6ebc..60ea02191b 100644 --- a/cartographer/io/proto_stream.cc +++ b/cartographer/io/proto_stream.cc @@ -15,6 +15,7 @@ */ #include "cartographer/io/proto_stream.h" + #include "glog/logging.h" namespace cartographer { diff --git a/cartographer/io/proto_stream_deserializer_test.cc b/cartographer/io/proto_stream_deserializer_test.cc index 44e5a0b6e6..d221a25f8b 100644 --- a/cartographer/io/proto_stream_deserializer_test.cc +++ b/cartographer/io/proto_stream_deserializer_test.cc @@ -14,10 +14,11 @@ * limitations under the License. */ +#include "cartographer/io/proto_stream_deserializer.h" + #include #include "cartographer/io/internal/in_memory_proto_stream.h" -#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/io/testing/test_helpers.h" #include "glog/logging.h" #include "gmock/gmock.h" diff --git a/cartographer/io/testing/test_helpers.cc b/cartographer/io/testing/test_helpers.cc index 1e9fbab911..00fcf57931 100644 --- a/cartographer/io/testing/test_helpers.cc +++ b/cartographer/io/testing/test_helpers.cc @@ -15,6 +15,7 @@ */ #include "cartographer/io/testing/test_helpers.h" + #include "cartographer/mapping/proto/serialization.pb.h" namespace cartographer { diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index 587573dbe4..1725334163 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -14,14 +14,13 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" - #include #include "absl/memory/memory.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/probability_values.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 3b9d41aa37..6b32b8abc4 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -15,7 +15,6 @@ */ #include "cartographer/mapping/2d/submap_2d.h" -#include "cartographer/mapping/2d/probability_grid.h" #include #include @@ -25,6 +24,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/transform/transform.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping/detect_floors.h b/cartographer/mapping/detect_floors.h index 189086f92c..b5acde8019 100644 --- a/cartographer/mapping/detect_floors.h +++ b/cartographer/mapping/detect_floors.h @@ -17,9 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ #define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ -#include "cartographer/mapping/proto/trajectory.pb.h" - #include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/2d/local_slam_result_2d.cc b/cartographer/mapping/internal/2d/local_slam_result_2d.cc index 05b078c954..d068d01456 100644 --- a/cartographer/mapping/internal/2d/local_slam_result_2d.cc +++ b/cartographer/mapping/internal/2d/local_slam_result_2d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/2d/local_slam_result_2d.h" + #include "cartographer/mapping/internal/2d/pose_graph_2d.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.h b/cartographer/mapping/internal/2d/normal_estimation_2d.h index f3d937de72..61e0a2f092 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d.h +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ #include + #include "cartographer/mapping/proto/2d/normal_estimation_options_2d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc index 29a076bc42..ce513a9c61 100644 --- a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc @@ -15,7 +15,6 @@ */ #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" - #include "cartographer/sensor/point_cloud.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc index 3cf182aa21..0c4422ee30 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" + #include "cartographer/mapping/probability_values.h" #include "ceres/cubic_interpolation.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc index 240e1c85c6..4cc82e266d 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -18,7 +18,6 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/probability_values.h" - #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index c25840fb50..5c6b5841c2 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -20,7 +20,6 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/tsdf_2d.h" #include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" - #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/3d/local_slam_result_3d.cc b/cartographer/mapping/internal/3d/local_slam_result_3d.cc index 614300852f..6f8e703def 100644 --- a/cartographer/mapping/internal/3d/local_slam_result_3d.cc +++ b/cartographer/mapping/internal/3d/local_slam_result_3d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/3d/local_slam_result_3d.h" + #include "cartographer/mapping/internal/3d/pose_graph_3d.h" namespace cartographer { diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index 066bb8c65e..f80fad9858 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -15,11 +15,11 @@ */ #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" -#include "cartographer/mapping/2d/probability_grid.h" #include #include "cartographer/common/internal/testing/thread_pool_for_testing.h" +#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder.h" #include "cartographer/mapping/internal/testing/test_helpers.h" diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 72a8e267ff..8e6db5de2c 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -17,11 +17,10 @@ #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ -#include "cartographer/mapping/map_builder_interface.h" - #include #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/sensor/collator_interface.h" diff --git a/cartographer/mapping/range_data_inserter_interface.cc b/cartographer/mapping/range_data_inserter_interface.cc index cf01396753..79d8c8d1bd 100644 --- a/cartographer/mapping/range_data_inserter_interface.cc +++ b/cartographer/mapping/range_data_inserter_interface.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/range_data_inserter_interface.h" + #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 0a6d9dfd48..4f547feb43 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -15,10 +15,10 @@ */ #include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/transform.h" #include +#include "cartographer/transform/transform.h" #include "gtest/gtest.h" namespace cartographer { From c999c3012b8b1abaf41935120219499a8f1ba872 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 10 Apr 2019 15:31:33 +0200 Subject: [PATCH 09/99] Fix a small description typo in constraints metrics. (#1562) --- cartographer/mapping/internal/3d/pose_graph_3d.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index a7e533e2be..049241a02e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1254,7 +1254,7 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) { kWorkQueueDelayMetric = latency->Add({}); auto* constraints = family_factory->NewGaugeFamily( "mapping_3d_pose_graph_constraints", - "Current number constraints in the pose graph"); + "Current number of constraints in the pose graph"); kConstraintsDifferentTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}}); kConstraintsSameTrajectoryMetric = From 897762675caffdb8f21d07ff01824528b56c2e8b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 10 Apr 2019 17:31:47 +0200 Subject: [PATCH 10/99] Use fixed precision for string histogram buckets. (#1563) `absl::StrCat/StrAppend` convert numeric values to strings with inconsistent decimal precision, which can lead to ugly formatting of the histogram. This can be fixed by using `StrAppendFormat` with `%f`, which uses 6 decimals (printf default and as it was when we had `std::to_string`). --- cartographer/common/histogram.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cartographer/common/histogram.cc b/cartographer/common/histogram.cc index e887208ea7..d54a9b5dab 100644 --- a/cartographer/common/histogram.cc +++ b/cartographer/common/histogram.cc @@ -21,6 +21,7 @@ #include #include "absl/strings/str_cat.h" +#include "absl/strings/str_format.h" #include "cartographer/common/port.h" #include "glog/logging.h" @@ -59,8 +60,8 @@ std::string Histogram::ToString(const int buckets) const { } } total_count += count; - absl::StrAppend(&result, "\n[", lower_bound, ", ", upper_bound, - i + 1 == buckets ? "]" : ")"); + absl::StrAppendFormat(&result, "\n[%f, %f%c", lower_bound, upper_bound, + i + 1 == buckets ? ']' : ')'); constexpr int kMaxBarChars = 20; const int bar = (count * kMaxBarChars + values_.size() / 2) / values_.size(); From 51df4afcba2dbb83e57ff34e04e8f01015f59625 Mon Sep 17 00:00:00 2001 From: Andre Gaschler Date: Fri, 12 Apr 2019 14:34:47 +0200 Subject: [PATCH 11/99] Update README.rst (#1566) --- README.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.rst b/README.rst index 9d66627a65..9916c89801 100644 --- a/README.rst +++ b/README.rst @@ -45,7 +45,7 @@ Open house We regularly meet in an open-for-all Google hangout to discuss progress and plans for Cartographer. You can join the `mailing list`_ to receive announcements. -The next Cartographer Open House Hangout is on **Thursday, April 11th 2019, 5pm CEST (8am PDT)** [`Hangouts link`_]. +The next Cartographer Open House Hangout is on **Thursday, May 9th 2019, 5pm CEST (8am PDT)** [`Hangouts link`_]. .. _mailing list: https://groups.google.com/forum/#!forum/google-cartographer .. _Hangouts link: https://staging.talkgadget.google.com/hangouts/_/google.com/cartographeropenhouse From bcd5486025df4f601c3977c44a5e00e9c80b4975 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Wed, 8 May 2019 17:21:22 +0200 Subject: [PATCH 12/99] Fix missing bazel dependency to absl/str_format.h in cartographer/common/histogram.cc (#1576) Also updates com_github_jupp0r_prometheus_cpp and bazel_skylib to fix compatibility issues with bazel 0.25 --- bazel/repositories.bzl | 11 +++++------ cartographer/BUILD.bazel | 3 ++- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index 15135941a3..8cdce51cad 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -206,9 +206,8 @@ def cartographer_repositories(): _maybe( http_archive, name = "bazel_skylib", - sha256 = "bbccf674aa441c266df9894182d80de104cabd19be98be002f6d478aaa31574d", - strip_prefix = "bazel-skylib-2169ae1c374aab4a09aa90e65efe1a3aad4e279b", - urls = ["https://github.com/bazelbuild/bazel-skylib/archive/2169ae1c374aab4a09aa90e65efe1a3aad4e279b.tar.gz"], + strip_prefix = "bazel-skylib-67215655bf6ce349b2b88ae4f5945a706f8ce959", + urls = ["https://github.com/bazelbuild/bazel-skylib/archive/67215655bf6ce349b2b88ae4f5945a706f8ce959.tar.gz"], ) _maybe( @@ -248,10 +247,10 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_jupp0r_prometheus_cpp", - sha256 = "6604ea0b5ef75f405c09218f13805d4141f6506eaf0da76f5f64625f62acfcd3", - strip_prefix = "prometheus-cpp-4e0814ee3f93b796356a51a4795a332568940a72", + sha256 = "07a704819cb90ed619cbf1a2713ba39faab27b8898b4561cc11a3c8b3ace83ea", + strip_prefix = "prometheus-cpp-4b11ee7a0aa7157494df06c4a324bf6d11bd0eec", urls = [ - "https://github.com/jupp0r/prometheus-cpp/archive/4e0814ee3f93b796356a51a4795a332568940a72.tar.gz", + "https://github.com/jupp0r/prometheus-cpp/archive/4b11ee7a0aa7157494df06c4a324bf6d11bd0eec.tar.gz", ], ) diff --git a/cartographer/BUILD.bazel b/cartographer/BUILD.bazel index 131fc1ce6e..33d54fdf2f 100644 --- a/cartographer/BUILD.bazel +++ b/cartographer/BUILD.bazel @@ -96,9 +96,10 @@ cc_library( ":cc_protos", "@boost//:iostreams", "@com_google_absl//absl/base", - "@com_google_absl//absl/strings", "@com_google_absl//absl/container:flat_hash_map", "@com_google_absl//absl/container:flat_hash_set", + "@com_google_absl//absl/strings", + "@com_google_absl//absl/strings:str_format", "@com_google_absl//absl/synchronization", "@com_google_absl//absl/types:optional", "@com_google_glog//:glog", From ad216d49c966a046073765207491ad1e479c4413 Mon Sep 17 00:00:00 2001 From: jie Date: Thu, 28 May 2020 10:28:15 -0700 Subject: [PATCH 13/99] Fix the dead loop. (#1586) Fix the bug in RotateHistogram which may cause dead loop by numerical error. --- .../internal/3d/scan_matching/rotational_scan_matcher.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 712cf897fa..0e4b5e0202 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -140,9 +140,13 @@ RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram) // rotations of a fractional bucket which is handled by linearly interpolating. Eigen::VectorXf RotationalScanMatcher::RotateHistogram( const Eigen::VectorXf& histogram, const float angle) { + if (histogram.size() == 0) { + return histogram; + } const float rotate_by_buckets = -angle * histogram.size() / M_PI; int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); const float fraction = rotate_by_buckets - full_buckets; + CHECK_GT(histogram.size(), 0); while (full_buckets < 0) { full_buckets += histogram.size(); } From 02594a07c76300439545c38004e15b1ae70dd5f4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 29 May 2020 14:09:03 +0200 Subject: [PATCH 14/99] Update README to archive Open House slides. (#1692) This moves the Cartographer Open House slides to the bottom of the README as an archive, and clarifies that regular meetings are currently no longer happening. --- README.rst | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/README.rst b/README.rst index 9916c89801..0f94e3e083 100644 --- a/README.rst +++ b/README.rst @@ -39,16 +39,19 @@ Getting started .. _our Read the Docs site: https://google-cartographer.readthedocs.io .. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question -Open house -========== +Contributing +============ -We regularly meet in an open-for-all Google hangout to discuss progress and plans for Cartographer. -You can join the `mailing list`_ to receive announcements. +You can find information about contributing to Cartographer at `our Contribution +page`_. -The next Cartographer Open House Hangout is on **Thursday, May 9th 2019, 5pm CEST (8am PDT)** [`Hangouts link`_]. +.. _our Contribution page: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md + +Open house slide archive +======================== -.. _mailing list: https://groups.google.com/forum/#!forum/google-cartographer -.. _Hangouts link: https://staging.talkgadget.google.com/hangouts/_/google.com/cartographeropenhouse +In the past there had been regular open-for-all meetings to discuss progress and plans for Cartographer. +Slides of these Cartographer Open House meetings are listed below. - March 14, 2019: `Slides `_ - February 21, 2019: `Slides `_ @@ -80,14 +83,6 @@ The next Cartographer Open House Hangout is on **Thursday, May 9th 2019, 5pm CES - June 22, 2017: `Slides `_ - June 8, 2017: `Slides `_ -Contributing -============ - -You can find information about contributing to Cartographer at `our Contribution -page`_. - -.. _our Contribution page: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md - .. |build| image:: https://travis-ci.org/googlecartographer/cartographer.svg?branch=master :alt: Build Status :scale: 100% From 47b80c9a3fd2685a99e4ef97fbe8ba7618f9f7d2 Mon Sep 17 00:00:00 2001 From: jie Date: Tue, 2 Jun 2020 02:29:37 -0700 Subject: [PATCH 15/99] Fix crashes in median() if all spans in a level are short (#1668) --- cartographer/mapping/detect_floors.cc | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/detect_floors.cc b/cartographer/mapping/detect_floors.cc index ad402ac846..8ec605dd80 100644 --- a/cartographer/mapping/detect_floors.cc +++ b/cartographer/mapping/detect_floors.cc @@ -188,8 +188,13 @@ std::vector FindFloors(const proto::Trajectory& trajectory, common::FromUniversal( trajectory.node(span.end_index - 1).timestamp())}); } - std::sort(z_values.begin(), z_values.end()); - floors.back().z = Median(z_values); + if (!z_values.empty()) { + std::sort(z_values.begin(), z_values.end()); + floors.back().z = Median(z_values); + } else { + LOG(ERROR) << "All spans in level are short"; + floors.pop_back(); + } } return floors; } From 1659f0dc8bcf6e55c4ac45f9baea822c7991bd38 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 2 Jun 2020 14:08:16 +0200 Subject: [PATCH 16/99] Fix build status on front page. (#1693) Changes "googlecartographer" to "cartographer-project" for references to CI and GitHub. --- README.rst | 12 ++++++------ RELEASING.rst | 2 +- bazel/repositories.bzl | 2 +- docs/source/index.rst | 6 +++--- docs/source/pbstream_migration.rst | 4 ++-- package.xml | 2 +- scripts/install_async_grpc.sh | 2 +- 7 files changed, 15 insertions(+), 15 deletions(-) diff --git a/README.rst b/README.rst index 0f94e3e083..1b216cbc17 100644 --- a/README.rst +++ b/README.rst @@ -27,7 +27,7 @@ configurations. |video| -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping Getting started @@ -37,7 +37,7 @@ Getting started * You can ask a question by `creating an issue`_. .. _our Read the Docs site: https://google-cartographer.readthedocs.io -.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question Contributing ============ @@ -45,7 +45,7 @@ Contributing You can find information about contributing to Cartographer at `our Contribution page`_. -.. _our Contribution page: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md +.. _our Contribution page: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md Open house slide archive ======================== @@ -83,10 +83,10 @@ Slides of these Cartographer Open House meetings are listed below. - June 22, 2017: `Slides `_ - June 8, 2017: `Slides `_ -.. |build| image:: https://travis-ci.org/googlecartographer/cartographer.svg?branch=master +.. |build| image:: https://travis-ci.org/cartographer-project/cartographer.svg?branch=master :alt: Build Status :scale: 100% - :target: https://travis-ci.org/googlecartographer/cartographer + :target: https://travis-ci.org/cartographer-project/cartographer .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest :alt: Documentation Status :scale: 100% @@ -94,7 +94,7 @@ Slides of these Cartographer Open House meetings are listed below. .. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg :alt: Apache 2 license. :scale: 100% - :target: https://github.com/googlecartographer/cartographer/blob/master/LICENSE + :target: https://github.com/cartographer-project/cartographer/blob/master/LICENSE .. |video| image:: https://j.gifs.com/wp3BJM.gif :alt: Cartographer 3D SLAM Demo :scale: 100% diff --git a/RELEASING.rst b/RELEASING.rst index 5a3a179606..19a87263f3 100644 --- a/RELEASING.rst +++ b/RELEASING.rst @@ -5,7 +5,7 @@ Steps for Releasing catkin_generate_changelog * Update changelog to point to GitHub release log (e.g. - https://github.com/googlecartographer/cartographer/compare/0.1.0...0.2.0) + https://github.com/cartographer-project/cartographer/compare/0.1.0...0.2.0) .. code-block:: bash git commit -am"Update changelog for release" diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index 8cdce51cad..c24bc91f38 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -260,7 +260,7 @@ def cartographer_repositories(): sha256 = "83c2a27c92979787f38810adc4b6bb67aa09607c53dbadca3430a5f29e0a1cd3", strip_prefix = "async_grpc-771af45374af7f7bfc3b622ed7efbe29a4aba403", urls = [ - "https://github.com/googlecartographer/async_grpc/archive/771af45374af7f7bfc3b622ed7efbe29a4aba403.tar.gz", + "https://github.com/cartographer-project/async_grpc/archive/771af45374af7f7bfc3b622ed7efbe29a4aba403.tar.gz", ], ) diff --git a/docs/source/index.rst b/docs/source/index.rst index 0ebfe5ccdd..b1a540b394 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -30,7 +30,7 @@ Cartographer and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor configurations. -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping Technical Overview @@ -38,7 +38,7 @@ Technical Overview * High level system overview of Cartographer .. image:: high_level_system_overview.png - :target: https://github.com/googlecartographer/cartographer/blob/master/docs/source/high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png .. To make modifications, edit the original Google Sketch and export a png. .. https://docs.google.com/drawings/d/1kCJ_dEbSvV83THCUfMikCPw7xFrTkrvRw5r6Ji8C90c/edit?usp=sharing @@ -56,7 +56,7 @@ ROS integration is provided by the `Cartographer ROS repository`_. You will find complete documentation for using Cartographer with ROS at the `Cartographer ROS Read the Docs site`_. -.. _Cartographer ROS repository: https://github.com/googlecartographer/cartographer_ros +.. _Cartographer ROS repository: https://github.com/cartographer-project/cartographer_ros .. _Cartographer ROS Read the Docs site: https://google-cartographer-ros.readthedocs.io Getting started without ROS diff --git a/docs/source/pbstream_migration.rst b/docs/source/pbstream_migration.rst index 14b9cee5c8..f2699f31b1 100644 --- a/docs/source/pbstream_migration.rst +++ b/docs/source/pbstream_migration.rst @@ -33,5 +33,5 @@ The tool assumes that the first pbstream provided as commandline argument, follows the serialization format of Cartographer 0.3. The resulting 1.0 pbstream will be saved to the second commandline argument location. -.. _RFC-0021: https://github.com/googlecartographer/rfcs/blob/master/text/0021-serialization-format.md -.. _source: https://github.com/googlecartographer/cartographer/blob/master/cartographer/io/pbstream_main.cc +.. _RFC-0021: https://github.com/cartographer-project/rfcs/blob/master/text/0021-serialization-format.md +.. _source: https://github.com/cartographer-project/cartographer/blob/master/cartographer/io/pbstream_main.cc diff --git a/package.xml b/package.xml index e50c112401..80b8981b4f 100644 --- a/package.xml +++ b/package.xml @@ -28,7 +28,7 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer + https://github.com/cartographer-project/cartographer The Cartographer Authors diff --git a/scripts/install_async_grpc.sh b/scripts/install_async_grpc.sh index 95ac84766a..73c8bbd8bc 100755 --- a/scripts/install_async_grpc.sh +++ b/scripts/install_async_grpc.sh @@ -17,7 +17,7 @@ set -o errexit set -o verbose -git clone https://github.com/googlecartographer/async_grpc +git clone https://github.com/cartographer-project/async_grpc cd async_grpc git checkout 771af45374af7f7bfc3b622ed7efbe29a4aba403 mkdir build From 46063be28f31e8e4951a8342b144346f3cc3899c Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 3 Jun 2020 10:36:59 +0200 Subject: [PATCH 17/99] MinMaxRangeFiteringPointsProcessor: use squaredNorm() instead of norm() (#1637) This should represent a considerable speedup. --- .../io/min_max_range_filtering_points_processor.cc | 7 ++++--- cartographer/io/min_max_range_filtering_points_processor.h | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index c487d6de9a..8cc56e7163 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -34,14 +34,15 @@ MinMaxRangeFiteringPointsProcessor::FromDictionary( MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( const double min_range, const double max_range, PointsProcessor* next) - : min_range_(min_range), max_range_(max_range), next_(next) {} + : min_range_squared_(min_range*min_range), max_range_squared_(max_range*max_range), + next_(next) {} void MinMaxRangeFiteringPointsProcessor::Process( std::unique_ptr batch) { absl::flat_hash_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { - const float range = (batch->points[i].position - batch->origin).norm(); - if (!(min_range_ <= range && range <= max_range_)) { + const float range = (batch->points[i].position - batch->origin).squaredNorm(); + if (!(min_range_squared_ <= range && range <= max_range_squared_)) { to_remove.insert(i); } } diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h index 2219cb61ba..5bc727cfe5 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.h +++ b/cartographer/io/min_max_range_filtering_points_processor.h @@ -47,8 +47,8 @@ class MinMaxRangeFiteringPointsProcessor : public PointsProcessor { FlushResult Flush() override; private: - const double min_range_; - const double max_range_; + const double min_range_squared_; + const double max_range_squared_; PointsProcessor* const next_; }; From 6bbf2558ce26ed74405f288d4c17df465f7eaeb1 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Wed, 3 Jun 2020 02:13:57 -0700 Subject: [PATCH 18/99] Drop superfluous ROS dependency on catkin (#1599) This package's CMakeLists.txt doesn't reference catkin at all, and can be regarded as a "pure CMake" package. In fact, the package.xml already states that this package is built using cmake directly, but still depends on catkin. This change makes the package buildable without catkin, as would be desired when building with colcon in ROS 2. --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 80b8981b4f..691be44ab5 100644 --- a/package.xml +++ b/package.xml @@ -34,7 +34,7 @@ The Cartographer Authors - catkin + cmake git google-mock From fe6e81a0555cc62f429f70fb96b2782aee82d3fe Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 3 Jun 2020 11:46:45 +0200 Subject: [PATCH 19/99] Fix typo, naming, formatting, type. (#1694) Fixes the typo mentioned in #1637. Also improves naming and ran clang-format. Changed an integer literal to unsigned to fix a compiler warning. --- ...in_max_range_filtering_points_processor.cc | 21 +++++++++++-------- ...min_max_range_filtering_points_processor.h | 18 ++++++++-------- .../io/points_processor_pipeline_builder.cc | 2 +- .../io/serialization_format_migration_test.cc | 4 ++-- 4 files changed, 24 insertions(+), 21 deletions(-) diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index 8cc56e7163..ba5fc79878 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -23,26 +23,29 @@ namespace cartographer { namespace io { -std::unique_ptr -MinMaxRangeFiteringPointsProcessor::FromDictionary( +std::unique_ptr +MinMaxRangeFilteringPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { - return absl::make_unique( + return absl::make_unique( dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), next); } -MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( +MinMaxRangeFilteringPointsProcessor::MinMaxRangeFilteringPointsProcessor( const double min_range, const double max_range, PointsProcessor* next) - : min_range_squared_(min_range*min_range), max_range_squared_(max_range*max_range), + : min_range_squared_(min_range * min_range), + max_range_squared_(max_range * max_range), next_(next) {} -void MinMaxRangeFiteringPointsProcessor::Process( +void MinMaxRangeFilteringPointsProcessor::Process( std::unique_ptr batch) { absl::flat_hash_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { - const float range = (batch->points[i].position - batch->origin).squaredNorm(); - if (!(min_range_squared_ <= range && range <= max_range_squared_)) { + const float range_squared = + (batch->points[i].position - batch->origin).squaredNorm(); + if (!(min_range_squared_ <= range_squared && + range_squared <= max_range_squared_)) { to_remove.insert(i); } } @@ -50,7 +53,7 @@ void MinMaxRangeFiteringPointsProcessor::Process( next_->Process(std::move(batch)); } -PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() { +PointsProcessor::FlushResult MinMaxRangeFilteringPointsProcessor::Flush() { return next_->Flush(); } diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h index 5bc727cfe5..0e90b3eee4 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.h +++ b/cartographer/io/min_max_range_filtering_points_processor.h @@ -27,21 +27,21 @@ namespace io { // Filters all points that are farther away from their 'origin' as 'max_range' // or closer than 'min_range'. -class MinMaxRangeFiteringPointsProcessor : public PointsProcessor { +class MinMaxRangeFilteringPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "min_max_range_filter"; - MinMaxRangeFiteringPointsProcessor(double min_range, double max_range, - PointsProcessor* next); - static std::unique_ptr FromDictionary( + MinMaxRangeFilteringPointsProcessor(double min_range, double max_range, + PointsProcessor* next); + static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); - ~MinMaxRangeFiteringPointsProcessor() override {} + ~MinMaxRangeFilteringPointsProcessor() override {} - MinMaxRangeFiteringPointsProcessor( - const MinMaxRangeFiteringPointsProcessor&) = delete; - MinMaxRangeFiteringPointsProcessor& operator=( - const MinMaxRangeFiteringPointsProcessor&) = delete; + MinMaxRangeFilteringPointsProcessor( + const MinMaxRangeFilteringPointsProcessor&) = delete; + MinMaxRangeFilteringPointsProcessor& operator=( + const MinMaxRangeFilteringPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 23c9232e4f..e50cc7730e 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -83,7 +83,7 @@ void RegisterBuiltInPointsProcessors( RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); - RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc index 24904ca63a..16e29b2bc3 100644 --- a/cartographer/io/serialization_format_migration_test.cc +++ b/cartographer/io/serialization_format_migration_test.cc @@ -150,7 +150,7 @@ TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) { mapping::proto::SerializationHeader header; EXPECT_TRUE(TextFormat::ParseFromString(output_messages_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1)); + EXPECT_THAT(header.format_version(), Eq(1u)); } TEST_F(MigrationTest, MigrationWithGridMigrationAddsHeaderAsFirstMessage) { @@ -165,7 +165,7 @@ TEST_F(MigrationTest, MigrationWithGridMigrationAddsHeaderAsFirstMessage) { mapping::proto::SerializationHeader header; EXPECT_TRUE(TextFormat::ParseFromString( output_messages_after_migrating_grid_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1)); + EXPECT_THAT(header.format_version(), Eq(1u)); } TEST_F(MigrationTest, SerializedDataOrderIsCorrect) { From 1b0d4f1feee7f3b7bdb64038e7392ba5f5b291b1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 4 Jun 2020 11:09:18 +0200 Subject: [PATCH 20/99] Add Ubuntu 18.04 to the install scripts, CI, docs. (#1697) --- .travis.yml | 1 + Dockerfile.bionic | 34 ++++++++++++++++++++++++++++++++++ docs/source/index.rst | 4 ++-- scripts/install_debs_cmake.sh | 6 ++++++ 4 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 Dockerfile.bionic diff --git a/.travis.yml b/.travis.yml index e81cd9f8c9..4d089cc9a1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,6 +24,7 @@ cache: env: - LSB_RELEASE=trusty DOCKER_CACHE_FILE=/home/travis/docker/trusty-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=jessie DOCKER_CACHE_FILE=/home/travis/docker/jessie-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ diff --git a/Dockerfile.bionic b/Dockerfile.bionic new file mode 100644 index 0000000000..1de82813dd --- /dev/null +++ b/Dockerfile.bionic @@ -0,0 +1,34 @@ +# Copyright 2020 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. + + +FROM ubuntu:bionic + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_proto3.sh cartographer/scripts/ +RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/docs/source/index.rst b/docs/source/index.rst index b1a540b394..54b04d2f12 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -93,8 +93,8 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 14.04 (Trusty) and 16.04 (Xenial) -* gcc version 4.8.4 and 5.4.0 +* Ubuntu 14.04 (Trusty), 16.04 (Xenial), 18.04 (Bionic) +* gcc version 4.8.4, 5.4.0, 7.5.0 Known Issues ------------ diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index 6a01bcb11a..db21f04b08 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -50,3 +50,9 @@ sudo apt-get install -y \ libsuitesparse-dev \ ninja-build \ python-sphinx + +# Install Ceres Solver on Ubuntu Bionic. No need to build it ourselves. +if [[ "$(lsb_release -sc)" = "bionic" ]] +then + sudo apt-get install -y libceres-dev +fi From 2f60378140a1b8229a68311f1c5383f30a379ecc Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 9 Jun 2020 12:35:47 +0200 Subject: [PATCH 21/99] Add VerticalRangeFilteringPointsProcessor. (#1636) Add an extra filter for 3D data. Solve the problem with 2D maps created from 3D sensors. --- .../io/points_processor_pipeline_builder.cc | 2 + ...rtical_range_filtering_points_processor.cc | 59 +++++++++++++++++++ ...ertical_range_filtering_points_processor.h | 58 ++++++++++++++++++ 3 files changed, 119 insertions(+) create mode 100644 cartographer/io/vertical_range_filtering_points_processor.cc create mode 100644 cartographer/io/vertical_range_filtering_points_processor.h diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index e50cc7730e..6592245b1e 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -29,6 +29,7 @@ #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer/io/vertical_range_filtering_points_processor.h" #include "cartographer/io/xray_points_processor.h" #include "cartographer/io/xyz_writing_points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -84,6 +85,7 @@ void RegisterBuiltInPointsProcessors( RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); diff --git a/cartographer/io/vertical_range_filtering_points_processor.cc b/cartographer/io/vertical_range_filtering_points_processor.cc new file mode 100644 index 0000000000..325d359cc6 --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.cc @@ -0,0 +1,59 @@ +/* + * Copyright 2016 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/io/vertical_range_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +VerticalRangeFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("min_z"), dictionary->GetDouble("max_z"), + next); +} + +VerticalRangeFilteringPointsProcessor::VerticalRangeFilteringPointsProcessor( + const double min_z, const double max_z, + PointsProcessor* next) + : min_z_(min_z), max_z_(max_z), + next_(next) {} + +void VerticalRangeFilteringPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const float distance_z = batch->points[i].position.z() - batch->origin.z(); + if (!(min_z_ <= distance_z && distance_z <= max_z_) ) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult VerticalRangeFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/vertical_range_filtering_points_processor.h b/cartographer/io/vertical_range_filtering_points_processor.h new file mode 100644 index 0000000000..d23eb20bed --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 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_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points which distance in the Z direction from their 'origin' +// exceeds 'max_z' or 'min_z'. +class VerticalRangeFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "vertical_range_filter"; + VerticalRangeFilteringPointsProcessor(double min_z, double max_z, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~VerticalRangeFilteringPointsProcessor() override {} + + VerticalRangeFilteringPointsProcessor( + const VerticalRangeFilteringPointsProcessor&) = delete; + VerticalRangeFilteringPointsProcessor& operator=( + const VerticalRangeFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double min_z_; + const double max_z_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ From db85e0894dea831904371e7fd89486bbbcb5abd6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 10 Jun 2020 11:13:34 +0200 Subject: [PATCH 22/99] Remove Ubuntu Trusty from CI. (#1699) Ubuntu 14.04 has reached end of standard support. Moves additional testing that was only done for Trusty to Bionic. --- .travis.yml | 1 - Dockerfile.bionic | 8 +++++++- Dockerfile.trusty | 37 ----------------------------------- cmake/modules/FindGMock.cmake | 2 -- docs/source/index.rst | 4 ++-- scripts/install_debs_cmake.sh | 7 ++----- 6 files changed, 11 insertions(+), 48 deletions(-) delete mode 100644 Dockerfile.trusty diff --git a/.travis.yml b/.travis.yml index 4d089cc9a1..35aa1021f1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,7 +22,6 @@ cache: - /home/travis/docker/ env: - - LSB_RELEASE=trusty DOCKER_CACHE_FILE=/home/travis/docker/trusty-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=jessie DOCKER_CACHE_FILE=/home/travis/docker/jessie-cache.tar.gz CC=gcc CXX=g++ diff --git a/Dockerfile.bionic b/Dockerfile.bionic index 1de82813dd..ff20efafd3 100644 --- a/Dockerfile.bionic +++ b/Dockerfile.bionic @@ -30,5 +30,11 @@ COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_proto3.sh cartographer/scripts/ RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY scripts/install_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_grpc.sh && rm -rf grpc +COPY scripts/install_async_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_async_grpc.sh && rm -rf async_grpc +COPY scripts/install_prometheus_cpp.sh cartographer/scripts/ +RUN cartographer/scripts/install_prometheus_cpp.sh && rm -rf prometheus-cpp COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer +RUN cartographer/scripts/install_cartographer_cmake_with_grpc.sh && rm -rf cartographer diff --git a/Dockerfile.trusty b/Dockerfile.trusty deleted file mode 100644 index ac96e93f9f..0000000000 --- a/Dockerfile.trusty +++ /dev/null @@ -1,37 +0,0 @@ -# Copyright 2016 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. - -FROM ubuntu:trusty - -ARG cc -ARG cxx - -# Set the preferred C/C++ compiler toolchain, if given (otherwise default). -ENV CC=$cc -ENV CXX=$cxx - -COPY scripts/install_debs_cmake.sh cartographer/scripts/ -RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf -COPY scripts/install_grpc.sh cartographer/scripts/ -RUN cartographer/scripts/install_grpc.sh && rm -rf grpc -COPY scripts/install_async_grpc.sh cartographer/scripts/ -RUN cartographer/scripts/install_async_grpc.sh && rm -rf async_grpc -COPY scripts/install_prometheus_cpp.sh cartographer/scripts/ -RUN cartographer/scripts/install_prometheus_cpp.sh && rm -rf prometheus-cpp -COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake_with_grpc.sh && rm -rf cartographer diff --git a/cmake/modules/FindGMock.cmake b/cmake/modules/FindGMock.cmake index c663e98368..952679e468 100644 --- a/cmake/modules/FindGMock.cmake +++ b/cmake/modules/FindGMock.cmake @@ -57,8 +57,6 @@ if(NOT GMock_FOUND) add_subdirectory(${GMOCK_SRC_DIR} "${CMAKE_CURRENT_BINARY_DIR}/gmock" EXCLUDE_FROM_ALL) endif() - # The next line is needed for Ubuntu Trusty. - set(GMOCK_INCLUDE_DIRS "${GMOCK_SRC_DIR}/gtest/include") set(GMOCK_LIBRARIES gmock_main) endif() endif() diff --git a/docs/source/index.rst b/docs/source/index.rst index 54b04d2f12..e466331284 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -65,7 +65,7 @@ Getting started without ROS Please see our ROS integration as a starting point for integrating your system with the standalone library. Currently, it is the best available reference. -On Ubuntu 14.04 (Trusty): +On Ubuntu 16.04 (Xenial): .. literalinclude:: ../../scripts/install_debs_cmake.sh :language: bash @@ -93,7 +93,7 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 14.04 (Trusty), 16.04 (Xenial), 18.04 (Bionic) +* Ubuntu 16.04 (Xenial), 18.04 (Bionic) * gcc version 4.8.4, 5.4.0, 7.5.0 Known Issues diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index db21f04b08..00d72d67f2 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -20,12 +20,9 @@ set -o verbose # Install the required libraries that are available as debs. sudo apt-get update -# Install CMake 3.2 for Ubuntu Trusty and Debian Jessie. +# Install CMake 3.2 for Debian Jessie. sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "trusty" ]] -then - sudo apt-get install cmake3 -y -elif [[ "$(lsb_release -sc)" = "jessie" ]] +if [[ "$(lsb_release -sc)" = "jessie" ]] then sudo sh -c "echo 'deb [check-valid-until=no] http://archive.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" sudo sh -c "echo 'Acquire::Check-Valid-Until \"false\";' >> /etc/apt/apt.conf" From d973a084f7e18568915178c7dd5cf1df67830925 Mon Sep 17 00:00:00 2001 From: Mariia Gladkova Date: Fri, 12 Jun 2020 12:17:54 +0200 Subject: [PATCH 23/99] Fix MapById::lower_bound function and implement test to reproduce the bug (#1616) This fixes the bug that has been faced during localization with an initial pose and a trimmed map (OverlappingSubmapsTrimmer2D has been used). A small test is added that reproduces the problem (an infinite loop) where a trimmed trajectory (MapById instance) is used. --- cartographer/mapping/id.h | 11 +++++++- cartographer/mapping/id_test.cc | 47 +++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 8e8cadc46e..c5abdf9af7 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -378,14 +378,23 @@ class MapById { const std::map& trajectory = trajectories_.at(trajectory_id).data_; + if (internal::GetTime(std::prev(trajectory.end())->second) < time) { return EndOfTrajectory(trajectory_id); } + auto left = trajectory.begin(); auto right = std::prev(trajectory.end()); while (left != right) { + // This is never 'right' which is important to guarantee progress. const int middle = left->first + (right->first - left->first) / 2; - const auto lower_bound_middle = trajectory.lower_bound(middle); + // This could be 'right' in the presence of gaps, so we need to use the + // previous element in this case. + auto lower_bound_middle = trajectory.lower_bound(middle); + if (lower_bound_middle->first > middle) { + CHECK(lower_bound_middle != left); + lower_bound_middle = std::prev(lower_bound_middle); + } if (internal::GetTime(lower_bound_middle->second) < time) { left = std::next(lower_bound_middle); } else { diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index 96a204b17c..ab877e0f52 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -254,6 +254,53 @@ TEST(IdTest, LowerBoundFuzz) { } } +TEST(IdTest, LowerBoundTrimmedTrajectory) { + constexpr int kTrajectoryId = 1; + + std::mt19937 rng; + std::uniform_int_distribution dt_dist(1, 20); + + const int N = 500; + int t = 0; + MapById map_by_id; + for (int j = 0; j < N; ++j) { + t = t + dt_dist(rng); + map_by_id.Append(kTrajectoryId, Data(t)); + } + + // Choose random length of a trim segment. + std::uniform_int_distribution dt_trim_segment_length( + 1, static_cast(N / 2)); + size_t trim_segment_length = dt_trim_segment_length(rng); + // Choose random start for a trim_segment. + std::uniform_int_distribution dt_trim_segment_start( + 2, N - trim_segment_length - 1); + size_t trim_segment_start_index = dt_trim_segment_start(rng); + + auto trim_segment_start = map_by_id.begin(); + std::advance(trim_segment_start, trim_segment_start_index); + + auto trim_segment_end = map_by_id.begin(); + std::advance(trim_segment_end, + trim_segment_start_index + trim_segment_length); + + for (auto it = trim_segment_start; it != trim_segment_end;) { + const auto this_it = it; + ++it; + map_by_id.Trim(this_it->id); + } + + auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(0)); + + auto ground_truth = + std::lower_bound(map_by_id.BeginOfTrajectory(kTrajectoryId), + map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(0), + [](MapById::IdDataReference a, + const common::Time& t) { return a.data.time() < t; }); + + EXPECT_EQ(ground_truth, it); +} + struct DataStruct { const common::Time time; }; From 36afcb55907d3a941a7bc91a90b277e083714e2c Mon Sep 17 00:00:00 2001 From: Phil Date: Mon, 15 Jun 2020 18:13:54 +1000 Subject: [PATCH 24/99] Fix typo in tsdf_range_data_inserter_2d.cc (#1612) Fix typo in RangeDataSorter. --- cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc index 491be9c438..d7619d089e 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc @@ -73,7 +73,7 @@ struct RangeDataSorter { const Eigen::Vector2f delta_lhs = (lhs.position.head<2>() - origin_).normalized(); const Eigen::Vector2f delta_rhs = - (lhs.position.head<2>() - origin_).normalized(); + (rhs.position.head<2>() - origin_).normalized(); if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) { return delta_lhs[1] < 0.f; } else if (delta_lhs[1] < 0.f) { From b38b1cefa02c3fb07df7a81c51fe5743c4552547 Mon Sep 17 00:00:00 2001 From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com> Date: Mon, 15 Jun 2020 10:56:23 +0200 Subject: [PATCH 25/99] Allow using TolerantLoss for INS in pose graph optimization (#1700) This PR adds the possibility to use the TolerantLoss function for INS in pose graph optimization. This is currently switched off, and so there's not functional change. The ceres TolerantLoss function (see [description](http://ceres-solver.org/nnls_modeling.html), and [implementation](https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/loss_function.cc)) has the following property - for large values of x it approaches a quadratic loss ("null loss") with the specified weight (fixed_frame_pose_translation_weight) - for small values of x it approaches a quadratic loss with a smaller weight - there's a crossover at some value x_c. The function is convex everywhere. --- .../mapping/internal/2d/pose_graph_2d_test.cc | 3 +++ .../optimization/optimization_problem_3d.cc | 5 ++++- .../optimization/optimization_problem_3d_test.cc | 3 +++ .../optimization/optimization_problem_options.cc | 6 ++++++ .../pose_graph/optimization_problem_options.proto | 13 +++++++++++-- configuration_files/pose_graph.lua | 3 +++ 6 files changed, 30 insertions(+), 3 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 965ff0a28d..924f36e78c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -142,6 +142,9 @@ class PoseGraph2DTest : public ::testing::Test { odometry_rotation_weight = 0., fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = true, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 1b8f63229a..83d1777f4e 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -548,7 +548,10 @@ void OptimizationProblem3D::Solve( problem.AddResidualBlock( SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose), - nullptr /* loss function */, + options_.fixed_frame_pose_use_tolerant_loss() ? + new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) : nullptr, C_fixed_frames.at(trajectory_id).rotation(), C_fixed_frames.at(trajectory_id).translation(), C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation()); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 4e3a1293b3..05bfac5d1c 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -48,6 +48,9 @@ class OptimizationProblem3DTest : public ::testing::Test { odometry_rotation_weight = 1e-2, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = true, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, diff --git a/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc index f32921727a..a17088ada9 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_options.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_options.cc @@ -42,6 +42,12 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); options.set_fixed_frame_pose_rotation_weight( parameter_dictionary->GetDouble("fixed_frame_pose_rotation_weight")); + options.set_fixed_frame_pose_use_tolerant_loss( + parameter_dictionary->GetBool("fixed_frame_pose_use_tolerant_loss")); + options.set_fixed_frame_pose_tolerant_loss_param_a( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_a")); + options.set_fixed_frame_pose_tolerant_loss_param_b( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_b")); options.set_log_solver_summary( parameter_dictionary->GetBool("log_solver_summary")); options.set_use_online_imu_extrinsics_in_3d( diff --git a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto index 6374fb66b0..efc61c184c 100644 --- a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto +++ b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto @@ -18,8 +18,9 @@ package cartographer.mapping.optimization.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 19 +// NEXT ID: 26 message OptimizationProblemOptions { + reserved 20 to 22; // For visual constraints. // Scaling parameter for Huber loss function. double huber_scale = 1; @@ -45,12 +46,20 @@ message OptimizationProblemOptions { // odometry. double odometry_rotation_weight = 17; - // Scaling parameter for the FixedFramePose translation. + // Scaling parameter for the FixedFramePose translation. Unit: 1/meters. double fixed_frame_pose_translation_weight = 11; // Scaling parameter for the FixedFramePose rotation. double fixed_frame_pose_rotation_weight = 12; + bool fixed_frame_pose_use_tolerant_loss = 23; + // The following parameters are used only if fixed_frame_pose_use_tolerant_loss is true. + // See http://ceres-solver.org/nnls_modeling.html. + // For large values of s, the tolerant loss function approaches a null loss + // with fixed_frame_pose_translation_weight. + double fixed_frame_pose_tolerant_loss_param_a = 24; + double fixed_frame_pose_tolerant_loss_param_b = 25; + // 3D only: fix Z. bool fix_z_in_3d = 13; diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index 797e407d66..572cd1a771 100644 --- a/configuration_files/pose_graph.lua +++ b/configuration_files/pose_graph.lua @@ -71,6 +71,9 @@ POSE_GRAPH = { odometry_rotation_weight = 1e5, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = false, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, From 10e57b0a278052e46e48491b57397c3ca4fd3220 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 16 Jun 2020 18:15:25 +0200 Subject: [PATCH 26/99] Change from Google CLA to DCO in CONTRIBUTING.md. (#1702) Signed-off-by: Wolfgang Hess --- CONTRIBUTING.md | 47 +++++++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2827b7d3fa..35894fc397 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,27 +1,30 @@ -Want to contribute? Great! First, read this page (including the small print at the end). +Want to contribute? Great! First, read this page. ### Before you contribute -Before we can use your code, you must sign the -[Google Individual Contributor License Agreement] -(https://cla.developers.google.com/about/google-individual) -(CLA), which you can do online. The CLA is necessary mainly because you own the -copyright to your changes, even after your contribution becomes part of our -codebase, so we need your permission to use and distribute your code. We also -need to be sure of various other things—for instance that you'll tell us if you -know that your code infringes on other people's patents. You don't have to sign -the CLA until after you've submitted your code for review and a member has -approved it, but you must do it before we can put your code into our codebase. -Before you start working on a larger contribution, you should get in touch with -us first through the issue tracker with your idea so that we can help out and -possibly guide you. Coordinating up front makes it much easier to avoid -frustration later on. + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. ### Code reviews -All submissions, including submissions by project members, require review. We -use Github pull requests for this purpose. -### The small print -Contributions made by corporations are covered by a different agreement than -the one above, the -[Software Grant and Corporate Contributor License Agreement] -(https://cla.developers.google.com/about/google-corporate). +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. From e8c1d840e661416e7f39b3ae26dd32b4f6ac07b9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 17 Jun 2020 10:09:14 +0200 Subject: [PATCH 27/99] Small fixes needed for focal support. (#1704) Signed-off-by: Wolfgang Hess --- cartographer/common/lockless_queue.h | 2 +- cartographer/mapping/3d/hybrid_grid_test.cc | 12 ++++++++---- cmake/modules/FindAbseil.cmake | 1 - 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/cartographer/common/lockless_queue.h b/cartographer/common/lockless_queue.h index 25174396ef..17c7ea0753 100644 --- a/cartographer/common/lockless_queue.h +++ b/cartographer/common/lockless_queue.h @@ -64,7 +64,7 @@ class LocklessQueue { data_list_head_ = data_list_head_->next; std::unique_ptr data = std::move(node->data); PushNodeToList(&free_list_head_, node); - return std::move(data); + return data; } return nullptr; } diff --git a/cartographer/mapping/3d/hybrid_grid_test.cc b/cartographer/mapping/3d/hybrid_grid_test.cc index 62309de912..333b114bbd 100644 --- a/cartographer/mapping/3d/hybrid_grid_test.cc +++ b/cartographer/mapping/3d/hybrid_grid_test.cc @@ -212,11 +212,15 @@ struct EigenComparator { TEST_F(RandomHybridGridTest, FromProto) { const HybridGrid constructed_grid(hybrid_grid_.ToProto()); - std::map member_map( - hybrid_grid_.begin(), hybrid_grid_.end()); + std::map member_map; + for (const auto& cell : hybrid_grid_) { + member_map.insert(cell); + } - std::map constructed_map( - constructed_grid.begin(), constructed_grid.end()); + std::map constructed_map; + for (const auto& cell : constructed_grid) { + constructed_map.insert(cell); + } EXPECT_EQ(member_map, constructed_map); } diff --git a/cmake/modules/FindAbseil.cmake b/cmake/modules/FindAbseil.cmake index 80d35c5e6d..14cd42b22e 100644 --- a/cmake/modules/FindAbseil.cmake +++ b/cmake/modules/FindAbseil.cmake @@ -52,7 +52,6 @@ if(NOT TARGET standalone_absl) "${ABSEIL_PROJECT_BUILD_DIR}/absl/meta/${prefix}absl_meta${suffix}" "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_int128${suffix}" "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_numeric${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/synchronization/${prefix}absl_synchronization${suffix}" "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_any${suffix}" "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_any_cast${suffix}" "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_optional_access${suffix}" From 82a3970f767929e5eb26b6fb3d69166ec94209ca Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 17 Jun 2020 10:59:39 +0200 Subject: [PATCH 28/99] Handle multiple frozen trajectories as one connected component. (#1610) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This fixes local constraint search in frozen maps built from multiple trajectories. Local constraint search from a localization trajectory to a set of frozen trajectories (i.e. the map) is done only among trajectories that are transitively connected in the same connected component. If we set an initial pose, we create such a connection to one frozen trajectory, but we were so far not able to do a local constraint search w.r.t. to other frozen trajectories because they're not connected among each other. Any constraints between them are not loaded (because they’re frozen). --- .../mapping/internal/2d/pose_graph_2d.cc | 16 ++++++++++++++++ .../mapping/internal/3d/pose_graph_3d.cc | 16 ++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 4c22be3e1b..b3620f5633 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -649,6 +649,22 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; }); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 049241a02e..cd470733ec 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -643,6 +643,22 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; }); From 21a8299caae4d2331551b757fa39de739ea64c45 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 18 Jun 2020 14:44:58 +0200 Subject: [PATCH 29/99] Add Ubuntu 20.04 to the install scripts, CI, docs. (#1706) Signed-off-by: Wolfgang Hess --- .travis.yml | 1 + Dockerfile.focal | 32 +++++++++++++++++++ .../2d/tsdf_range_data_inserter_2d_test.cc | 4 +-- cmake/modules/FindGMock.cmake | 19 +++++++++++ docs/source/index.rst | 4 +-- scripts/install_debs_cmake.sh | 16 ++++++++-- 6 files changed, 70 insertions(+), 6 deletions(-) create mode 100644 Dockerfile.focal diff --git a/.travis.yml b/.travis.yml index 35aa1021f1..5ebfb69d1b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,6 +24,7 @@ cache: env: - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=jessie DOCKER_CACHE_FILE=/home/travis/docker/jessie-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ diff --git a/Dockerfile.focal b/Dockerfile.focal new file mode 100644 index 0000000000..c443c1883e --- /dev/null +++ b/Dockerfile.focal @@ -0,0 +1,32 @@ +# Copyright 2020 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. + + +FROM ubuntu:focal + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc index 4db939a016..7e59508bda 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc @@ -27,7 +27,7 @@ namespace { class RangeDataInserterTest2DTSDF : public ::testing::Test { protected: RangeDataInserterTest2DTSDF() - : tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0, + : tsdf_(MapLimits(1., Eigen::Vector2d(1., 7.), CellLimits(8, 1)), 2.0, 10.0, &conversion_tables_) { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -346,4 +346,4 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) { } // namespace } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer diff --git a/cmake/modules/FindGMock.cmake b/cmake/modules/FindGMock.cmake index 952679e468..76cea16161 100644 --- a/cmake/modules/FindGMock.cmake +++ b/cmake/modules/FindGMock.cmake @@ -30,6 +30,25 @@ if(NOT GMock_FOUND) PATHS /usr ) + if(GMOCK_LIBRARIES) + find_library(GMOCK_LIBRARY + NAMES gmock + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + find_library(GTEST_LIBRARY + NAMES gtest + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + list(APPEND GMOCK_LIBRARIES ${GMOCK_LIBRARY} ${GTEST_LIBRARY}) + endif() # Find system-wide gtest header. find_path(GTEST_INCLUDE_DIRS gtest/gtest.h diff --git a/docs/source/index.rst b/docs/source/index.rst index e466331284..bdb8621e05 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -93,8 +93,8 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 16.04 (Xenial), 18.04 (Bionic) -* gcc version 4.8.4, 5.4.0, 7.5.0 +* Ubuntu 16.04 (Xenial), 18.04 (Bionic), 20.04 (Focal) +* gcc version 4.8.4, 5.4.0, 7.5.0, 9.3.0 Known Issues ------------ diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index 00d72d67f2..fe71846ab2 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -45,11 +45,23 @@ sudo apt-get install -y \ libgoogle-glog-dev \ liblua5.2-dev \ libsuitesparse-dev \ - ninja-build \ - python-sphinx + ninja-build + +if [[ "$(lsb_release -sc)" = "focal" ]] +then + sudo apt-get install -y python3-sphinx libgmock-dev +else + sudo apt-get install -y python-sphinx +fi # Install Ceres Solver on Ubuntu Bionic. No need to build it ourselves. if [[ "$(lsb_release -sc)" = "bionic" ]] then sudo apt-get install -y libceres-dev fi + +# Install Ceres Solver and Protocol Buffers support on Ubuntu Focal. No need to build it ourselves. +if [[ "$(lsb_release -sc)" = "focal" ]] +then + sudo apt-get install -y libceres-dev protobuf-compiler +fi From a5aa03b776220c5270dbddec359f471af59799b9 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 23 Jun 2020 10:34:01 +0200 Subject: [PATCH 30/99] Implement fixed size transform interpolation buffer. (#1581) --- .../transform_interpolation_buffer.cc | 23 ++++++++++++++++ .../transform_interpolation_buffer.h | 26 ++++++++++++++++--- .../transform_interpolation_buffer_test.cc | 16 ++++++++++++ 3 files changed, 62 insertions(+), 3 deletions(-) diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc index 7d9c3d7d78..ae65721c19 100644 --- a/cartographer/transform/transform_interpolation_buffer.cc +++ b/cartographer/transform/transform_interpolation_buffer.cc @@ -40,8 +40,17 @@ void TransformInterpolationBuffer::Push(const common::Time time, CHECK_GE(time, latest_time()) << "New transform is older than latest."; } timestamped_transforms_.push_back(TimestampedTransform{time, transform}); + RemoveOldTransformsIfNeeded(); } +void TransformInterpolationBuffer::SetSizeLimit( + const size_t buffer_size_limit) { + buffer_size_limit_ = buffer_size_limit; + RemoveOldTransformsIfNeeded(); +} + +void TransformInterpolationBuffer::Clear() { timestamped_transforms_.clear(); } + bool TransformInterpolationBuffer::Has(const common::Time time) const { if (timestamped_transforms_.empty()) { return false; @@ -65,6 +74,12 @@ transform::Rigid3d TransformInterpolationBuffer::Lookup( return Interpolate(*start, *end, time).transform; } +void TransformInterpolationBuffer::RemoveOldTransformsIfNeeded() { + while (timestamped_transforms_.size() > buffer_size_limit_) { + timestamped_transforms_.pop_front(); + } +} + common::Time TransformInterpolationBuffer::earliest_time() const { CHECK(!empty()) << "Empty buffer."; return timestamped_transforms_.front().time; @@ -79,5 +94,13 @@ bool TransformInterpolationBuffer::empty() const { return timestamped_transforms_.empty(); } +size_t TransformInterpolationBuffer::size_limit() const { + return buffer_size_limit_; +} + +size_t TransformInterpolationBuffer::size() const { + return timestamped_transforms_.size(); +} + } // namespace transform } // namespace cartographer diff --git a/cartographer/transform/transform_interpolation_buffer.h b/cartographer/transform/transform_interpolation_buffer.h index 4dc51bb9d4..84a7dcd2b4 100644 --- a/cartographer/transform/transform_interpolation_buffer.h +++ b/cartographer/transform/transform_interpolation_buffer.h @@ -17,7 +17,8 @@ #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ -#include +#include +#include #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -27,18 +28,28 @@ namespace cartographer { namespace transform { +constexpr size_t kUnlimitedBufferSize = std::numeric_limits::max(); + // A time-ordered buffer of transforms that supports interpolated lookups. +// Unless explicitly set, the buffer size is unlimited. class TransformInterpolationBuffer { public: TransformInterpolationBuffer() = default; explicit TransformInterpolationBuffer( const mapping::proto::Trajectory& trajectory); + // Sets the transform buffer size limit and removes old transforms + // if it is exceeded. + void SetSizeLimit(size_t buffer_size_limit); + // Adds a new transform to the buffer and removes the oldest transform if the // buffer size limit is exceeded. void Push(common::Time time, const transform::Rigid3d& transform); - // Returns true if an interpolated transfrom can be computed at 'time'. + // Clears the transform buffer. + void Clear(); + + // Returns true if an interpolated transform can be computed at 'time'. bool Has(common::Time time) const; // Returns an interpolated transform at 'time'. CHECK()s that a transform at @@ -56,8 +67,17 @@ class TransformInterpolationBuffer { // Returns true if the buffer is empty. bool empty() const; + // Returns the maximum allowed size of the transform buffer. + size_t size_limit() const; + + // Returns the current size of the transform buffer. + size_t size() const; + private: - std::vector timestamped_transforms_; + void RemoveOldTransformsIfNeeded(); + + std::deque timestamped_transforms_; + size_t buffer_size_limit_ = kUnlimitedBufferSize; }; } // namespace transform diff --git a/cartographer/transform/transform_interpolation_buffer_test.cc b/cartographer/transform/transform_interpolation_buffer_test.cc index 193f95e419..b78d5692ec 100644 --- a/cartographer/transform/transform_interpolation_buffer_test.cc +++ b/cartographer/transform/transform_interpolation_buffer_test.cc @@ -70,6 +70,22 @@ TEST(TransformInterpolationBufferTest, testLookupSingleTransform) { EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6)); } +TEST(TransformInterpolationBufferTest, testSetSizeLimit) { + TransformInterpolationBuffer buffer; + EXPECT_EQ(buffer.size_limit(), kUnlimitedBufferSize); + buffer.Push(common::FromUniversal(0), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(1), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(2), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 3); + buffer.SetSizeLimit(2); + EXPECT_EQ(buffer.size_limit(), 2); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(0))); + buffer.Push(common::FromUniversal(3), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(1))); +} + } // namespace } // namespace transform } // namespace cartographer From ade7605facb967ae7adcbab4493eab4ead9454c6 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Wed, 24 Jun 2020 10:53:03 +0200 Subject: [PATCH 31/99] Use installed Abseil. (#1570) --- CMakeLists.txt | 17 +++++- Dockerfile.bionic | 2 + Dockerfile.focal | 2 + Dockerfile.jessie | 2 + Dockerfile.stretch | 2 + Dockerfile.xenial | 2 + cmake/modules/FindAbseil.cmake | 95 ---------------------------------- scripts/install_abseil.sh | 32 ++++++++++++ scripts/install_debs_cmake.sh | 3 +- 9 files changed, 59 insertions(+), 98 deletions(-) delete mode 100644 cmake/modules/FindAbseil.cmake create mode 100755 scripts/install_abseil.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index de3645d5cd..ef2fcb3e69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ include("${PROJECT_SOURCE_DIR}/cmake/functions.cmake") google_initialize_cartographer_project() google_enable_testing() -find_package(Abseil REQUIRED) +find_package(absl REQUIRED) set(BOOST_COMPONENTS iostreams) if(WIN32) list(APPEND BOOST_COMPONENTS zlib) @@ -268,7 +268,20 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PROTOBUF_INCLUDE_DIR}) # TODO(hrapp): This should not explicitly list pthread and use # PROTOBUF_LIBRARIES, but that failed on first try. -target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} standalone_absl) +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} + absl::algorithm + absl::base + absl::debugging + absl::flat_hash_map + absl::memory + absl::meta + absl::numeric + absl::str_format + absl::strings + absl::synchronization + absl::time + absl::utility +) if (NOT WIN32) target_link_libraries(${PROJECT_NAME} PUBLIC pthread) endif() diff --git a/Dockerfile.bionic b/Dockerfile.bionic index ff20efafd3..e65606da99 100644 --- a/Dockerfile.bionic +++ b/Dockerfile.bionic @@ -28,6 +28,8 @@ RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/li COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_proto3.sh cartographer/scripts/ RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf COPY scripts/install_grpc.sh cartographer/scripts/ diff --git a/Dockerfile.focal b/Dockerfile.focal index c443c1883e..a2f7aed278 100644 --- a/Dockerfile.focal +++ b/Dockerfile.focal @@ -28,5 +28,7 @@ RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/li COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY . cartographer RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.jessie b/Dockerfile.jessie index aca374222c..0908e62c4c 100644 --- a/Dockerfile.jessie +++ b/Dockerfile.jessie @@ -26,6 +26,8 @@ RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_ceres.sh cartographer/scripts/ RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver COPY scripts/install_proto3.sh cartographer/scripts/ diff --git a/Dockerfile.stretch b/Dockerfile.stretch index 91d52117b7..b3f346e78d 100644 --- a/Dockerfile.stretch +++ b/Dockerfile.stretch @@ -26,6 +26,8 @@ RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_ceres.sh cartographer/scripts/ RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver COPY scripts/install_proto3.sh cartographer/scripts/ diff --git a/Dockerfile.xenial b/Dockerfile.xenial index 715287b10f..43231d5a0d 100644 --- a/Dockerfile.xenial +++ b/Dockerfile.xenial @@ -27,6 +27,8 @@ RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_ceres.sh cartographer/scripts/ RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver COPY scripts/install_proto3.sh cartographer/scripts/ diff --git a/cmake/modules/FindAbseil.cmake b/cmake/modules/FindAbseil.cmake deleted file mode 100644 index 14cd42b22e..0000000000 --- a/cmake/modules/FindAbseil.cmake +++ /dev/null @@ -1,95 +0,0 @@ -# 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. - -cmake_minimum_required(VERSION 3.2) - -if(NOT TARGET standalone_absl) - set(prefix ${CMAKE_STATIC_LIBRARY_PREFIX}) - set(suffix ${CMAKE_STATIC_LIBRARY_SUFFIX}) - include(${CMAKE_ROOT}/Modules/ExternalProject.cmake) - set(ABSEIL_PROJECT_NAME abseil) - set(ABSEIL_PROJECT_SRC_DIR - ${CMAKE_CURRENT_BINARY_DIR}/${ABSEIL_PROJECT_NAME}/src/${ABSEIL_PROJECT_NAME}) - set(ABSEIL_PROJECT_BUILD_DIR - ${CMAKE_CURRENT_BINARY_DIR}/${ABSEIL_PROJECT_NAME}/src/${ABSEIL_PROJECT_NAME}-build) - set(ABSEIL_INCLUDE_DIRS ${ABSEIL_PROJECT_SRC_DIR}) - set(ABSEIL_LIBRARY_PATH - "${ABSEIL_PROJECT_BUILD_DIR}/absl/synchronization/${prefix}absl_synchronization${suffix}") - set(ABSEIL_DEPENDENT_LIBRARIES - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_symbolize${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/time/${prefix}absl_time${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}str_format_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}str_format_extension_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}absl_str_format${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}absl_strings${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/hash/${prefix}absl_hash${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/algorithm/${prefix}absl_algorithm${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_base${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_dynamic_annotations${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_malloc_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_spinlock_wait${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_throw_delegate${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/container/${prefix}absl_container${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/container/${prefix}test_instance_tracker_lib${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_debugging${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_examine_stack${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_failure_signal_handler${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_leak_check${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_stack_consumption${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_stacktrace${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/memory/${prefix}absl_memory${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/meta/${prefix}absl_meta${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_int128${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_numeric${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_any${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_any_cast${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_optional_access${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_optional${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_span${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_variant${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/utility/${prefix}absl_utility${suffix}" - ) - ExternalProject_Add(${ABSEIL_PROJECT_NAME} - PREFIX ${ABSEIL_PROJECT_NAME} - GIT_REPOSITORY https://github.com/abseil/abseil-cpp.git - GIT_TAG 7b46e1d31a6b08b1c6da2a13e7b151a20446fa07 - INSTALL_COMMAND "" - BUILD_COMMAND ${CMAKE_COMMAND} --build "${ABSEIL_PROJECT_BUILD_DIR}" - CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON;-DBUILD_TESTING:BOOL=OFF;-DCMAKE_BUILD_TYPE:STRING=Release" - BUILD_BYPRODUCTS "${ABSEIL_LIBRARY_PATH};${ABSEIL_DEPENDENT_LIBRARIES}" - ) - add_library(standalone_absl STATIC IMPORTED GLOBAL) - set_target_properties(standalone_absl - PROPERTIES INTERFACE_INCLUDE_DIRECTORIES - ${ABSEIL_INCLUDE_DIRS} - ) - set_target_properties(standalone_absl - PROPERTIES IMPORTED_LOCATION - ${ABSEIL_LIBRARY_PATH} - INTERFACE_LINK_LIBRARIES - "${ABSEIL_DEPENDENT_LIBRARIES}" - ) - if(MSVC) - # /wd4005 macro-redefinition - # /wd4068 unknown pragma - # /wd4244 conversion from 'type1' to 'type2' - # /wd4267 conversion from 'size_t' to 'type2' - # /wd4800 force value to bool 'true' or 'false' (performance warning) - target_compile_options(standalone_absl INTERFACE /wd4005 /wd4068 /wd4244 /wd4267 /wd4800) - target_compile_definitions(standalone_absl INTERFACE -DNOMINMAX -DWIN32_LEAN_AND_MEAN=1 -D_CRT_SECURE_NO_WARNINGS) - endif() - add_dependencies(standalone_absl ${ABSEIL_PROJECT_NAME}) - unset(prefix) - unset(suffix) -endif() diff --git a/scripts/install_abseil.sh b/scripts/install_abseil.sh new file mode 100755 index 0000000000..f7aafb02ab --- /dev/null +++ b/scripts/install_abseil.sh @@ -0,0 +1,32 @@ +#!/bin/sh + +# Copyright 2019 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. + +set -o errexit +set -o verbose + +git clone https://github.com/abseil/abseil-cpp.git +cd abseil-cpp +git checkout d902eb869bcfacc1bad14933ed9af4bed006d481 +mkdir build +cd build +cmake -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local/stow/absl \ + .. +ninja +sudo ninja install +cd /usr/local/stow +sudo stow absl diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index fe71846ab2..bc8649ec2b 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -45,7 +45,8 @@ sudo apt-get install -y \ libgoogle-glog-dev \ liblua5.2-dev \ libsuitesparse-dev \ - ninja-build + ninja-build \ + stow if [[ "$(lsb_release -sc)" = "focal" ]] then From e5894cce1f8047d5c807158711e468b3f5550f1a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 25 Jun 2020 09:35:57 +0200 Subject: [PATCH 32/99] Small Abseil fixes for dependents. (#1711) Enable PIC so that shared libraries can use the Cartographer library. Fixes the config so that dependent builds can find Abseil. Signed-off-by: Wolfgang Hess --- cartographer-config.cmake.in | 2 +- docs/source/index.rst | 4 ++++ scripts/install_abseil.sh | 1 + 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/cartographer-config.cmake.in b/cartographer-config.cmake.in index 1986207a53..98193da912 100644 --- a/cartographer-config.cmake.in +++ b/cartographer-config.cmake.in @@ -39,7 +39,7 @@ find_package(Ceres ${QUIET_OR_REQUIRED_OPTION} HINTS ${CERES_DIR_HINTS}) if (WIN32) find_package(glog REQUIRED) endif() -find_package(Abseil ${QUIET_OR_REQUIRED_OPTION}) +find_package(absl ${QUIET_OR_REQUIRED_OPTION}) if(CARTOGRAPHER_HAS_GRPC) find_package(async_grpc ${QUIET_OR_REQUIRED_OPTION}) endif() diff --git a/docs/source/index.rst b/docs/source/index.rst index bdb8621e05..8de158acbb 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -71,6 +71,10 @@ On Ubuntu 16.04 (Xenial): :language: bash :lines: 20- +.. literalinclude:: ../../scripts/install_abseil.sh + :language: bash + :lines: 20- + .. literalinclude:: ../../scripts/install_ceres.sh :language: bash :lines: 20- diff --git a/scripts/install_abseil.sh b/scripts/install_abseil.sh index f7aafb02ab..4962d6ecc9 100755 --- a/scripts/install_abseil.sh +++ b/scripts/install_abseil.sh @@ -24,6 +24,7 @@ mkdir build cd build cmake -G Ninja \ -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DCMAKE_INSTALL_PREFIX=/usr/local/stow/absl \ .. ninja From d7016fdbbc256e67fc84e5f523f9cb1c4de41bfb Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 6 Jul 2020 16:08:49 +0200 Subject: [PATCH 33/99] Remove Debian Jessie from CI. (#1713) It has reached end-of-life with the end of LTS on June 30, 2020. Signed-off-by: Wolfgang Hess --- .travis.yml | 1 - Dockerfile.jessie | 36 ----------------------------------- scripts/install_debs_cmake.sh | 15 ++------------- 3 files changed, 2 insertions(+), 50 deletions(-) delete mode 100644 Dockerfile.jessie diff --git a/.travis.yml b/.travis.yml index 5ebfb69d1b..f6a4e96299 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,7 +25,6 @@ env: - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=jessie DOCKER_CACHE_FILE=/home/travis/docker/jessie-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ before_install: scripts/load_docker_cache.sh diff --git a/Dockerfile.jessie b/Dockerfile.jessie deleted file mode 100644 index 0908e62c4c..0000000000 --- a/Dockerfile.jessie +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright 2016 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. - -FROM debian:jessie - -ARG cc -ARG cxx - -# Set the preferred C/C++ compiler toolchain, if given (otherwise default). -ENV CC=$cc -ENV CXX=$cxx - -# This base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* - -COPY scripts/install_debs_cmake.sh cartographer/scripts/ -RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_abseil.sh cartographer/scripts/ -RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf -COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index bc8649ec2b..aed400f643 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -19,21 +19,9 @@ set -o verbose # Install the required libraries that are available as debs. sudo apt-get update - -# Install CMake 3.2 for Debian Jessie. -sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "jessie" ]] -then - sudo sh -c "echo 'deb [check-valid-until=no] http://archive.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" - sudo sh -c "echo 'Acquire::Check-Valid-Until \"false\";' >> /etc/apt/apt.conf" - sudo apt-get update - sudo apt-get -t jessie-backports install cmake -y -else - sudo apt-get install cmake -y -fi - sudo apt-get install -y \ clang \ + cmake \ g++ \ git \ google-mock \ @@ -45,6 +33,7 @@ sudo apt-get install -y \ libgoogle-glog-dev \ liblua5.2-dev \ libsuitesparse-dev \ + lsb-release \ ninja-build \ stow From 5899bff425bb8831e7cf2b84b610cc65fa0b4cd9 Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Tue, 7 Jul 2020 20:18:53 +0900 Subject: [PATCH 34/99] FixedFramePose in optimization 2d (#1580) --- .../mapping/internal/2d/pose_graph_2d.cc | 37 +++++-- .../optimization/optimization_problem_2d.cc | 96 +++++++++++++++++++ .../optimization/optimization_problem_2d.h | 17 ++++ 3 files changed, 143 insertions(+), 7 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index b3620f5633..6ccf43bc6f 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -229,7 +229,14 @@ void PoseGraph2D::AddOdometryData(const int trajectory_id, void PoseGraph2D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { - LOG(FATAL) << "Not yet implemented for 2D."; + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddFixedFramePoseData(trajectory_id, + fixed_frame_pose_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph2D::AddLandmarkData(int trajectory_id, @@ -753,7 +760,24 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph2D::SetTrajectoryDataFromProto( const proto::TrajectoryData& data) { - LOG(ERROR) << "not implemented"; + TrajectoryData trajectory_data; + // gravity_constant and imu_calibration are omitted as its not used in 2d + + if (data.has_fixed_frame_origin_in_map()) { + trajectory_data.fixed_frame_origin_in_map = + transform::ToRigid3(data.fixed_frame_origin_in_map()); + + const int trajectory_id = data.trajectory_id(); + AddWorkItem([this, trajectory_id, trajectory_data]() + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->SetTrajectoryData( + trajectory_id, trajectory_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); + } } void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, @@ -984,15 +1008,14 @@ PoseGraph2D::GetLandmarkNodes() const { std::map PoseGraph2D::GetTrajectoryData() const { - // The 2D optimization problem does not have any 'TrajectoryData'. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->trajectory_data(); } sensor::MapByTime PoseGraph2D::GetFixedFramePoseData() const { - // FixedFramePoseData is not yet implemented for 2D. We need to return empty - // so serialization works. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->fixed_frame_pose_data(); } std::vector PoseGraph2D::constraints() const { diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index cd0eafc99b..508cae7cd3 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -42,6 +42,35 @@ namespace { using ::cartographer::mapping::optimization::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using TrajectoryData = + ::cartographer::mapping::PoseGraphInterface::TrajectoryData; + +// For fixed frame pose. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id) || + !it->pose.has_value()) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose.value()); + } + return nullptr; + } + const auto prev_it = std::prev(it); + if (prev_it->pose.has_value()) { + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, + prev_it->pose.value()}, + transform::TimestampedTransform{it->time, it->pose.value()}, + time) + .transform); + } + return nullptr; +} // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the @@ -154,20 +183,36 @@ void OptimizationProblem2D::AddOdometryData( odometry_data_.Append(trajectory_id, odometry_data); } +void OptimizationProblem2D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); +} + void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, const NodeSpec2D& node_data) { node_data_.Append(trajectory_id, node_data); + trajectory_data_[trajectory_id]; +} + +void OptimizationProblem2D::SetTrajectoryData( + int trajectory_id, const TrajectoryData& trajectory_data) { + trajectory_data_[trajectory_id] = trajectory_data; } void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, const NodeSpec2D& node_data) { node_data_.Insert(node_id, node_data); + trajectory_data_[node_id.trajectory_id]; } void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); + if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { + trajectory_data_.erase(node_id.trajectory_id); + } } void OptimizationProblem2D::AddSubmap( @@ -301,6 +346,53 @@ void OptimizationProblem2D::Solve( } } + std::map> C_fixed_frames; + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) { + node_it = trajectory_end; + continue; + } + + const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + bool fixed_frame_pose_initialized = false; + for (; node_it != trajectory_end; ++node_it) { + const NodeId node_id = node_it->id; + const NodeSpec2D& node_data = node_it->data; + + const std::unique_ptr fixed_frame_pose = + Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); + if (fixed_frame_pose == nullptr) { + continue; + } + + const Constraint::Pose constraint_pose{ + *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), + options_.fixed_frame_pose_rotation_weight()}; + + if (!fixed_frame_pose_initialized) { + transform::Rigid2d fixed_frame_pose_in_map; + if (trajectory_data.fixed_frame_origin_in_map.has_value()) { + fixed_frame_pose_in_map = transform::Project2D( + trajectory_data.fixed_frame_origin_in_map.value()); + } else { + fixed_frame_pose_in_map = node_data.global_pose_2d; + } + + C_fixed_frames.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple(FromPose(fixed_frame_pose_in_map))); + fixed_frame_pose_initialized = true; + } + + problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose), + nullptr /* loss function */, + C_fixed_frames.at(trajectory_id).data(), + C_nodes.at(node_id).data()); + } + } + // Solve. ceres::Solver::Summary summary; ceres::Solve( @@ -319,6 +411,10 @@ void OptimizationProblem2D::Solve( node_data_.at(C_node_id_data.id).global_pose_2d = ToPose(C_node_id_data.data); } + for (const auto& C_fixed_frame : C_fixed_frames) { + trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map = + transform::Embed3D(ToPose(C_fixed_frame.second)); + } for (const auto& C_landmark : C_landmarks) { landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); } diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h index 496f2d9a9a..f8312317db 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -101,6 +101,21 @@ class OptimizationProblem2D return odometry_data_; } + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); + void SetTrajectoryData( + int trajectory_id, + const PoseGraphInterface::TrajectoryData& trajectory_data); + const sensor::MapByTime& fixed_frame_pose_data() + const { + return fixed_frame_pose_data_; + } + const std::map& trajectory_data() + const { + return trajectory_data_; + } + private: std::unique_ptr InterpolateOdometry( int trajectory_id, common::Time time) const; @@ -115,6 +130,8 @@ class OptimizationProblem2D std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; + sensor::MapByTime fixed_frame_pose_data_; + std::map trajectory_data_; }; } // namespace optimization From 595ffba0cea39945a02fd071065402aee00f7a81 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 8 Jul 2020 14:50:02 +0200 Subject: [PATCH 35/99] Small improvements to 2D fixed frame pose support. (#1714) This is a follow up to #1580. Signed-off-by: Wolfgang Hess --- .../optimization/optimization_problem_2d.cc | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index 508cae7cd3..b2ca862e8a 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -209,6 +209,7 @@ void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); + fixed_frame_pose_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { trajectory_data_.erase(node_id.trajectory_id); @@ -377,19 +378,24 @@ void OptimizationProblem2D::Solve( fixed_frame_pose_in_map = transform::Project2D( trajectory_data.fixed_frame_origin_in_map.value()); } else { - fixed_frame_pose_in_map = node_data.global_pose_2d; + fixed_frame_pose_in_map = + node_data.global_pose_2d * + transform::Project2D(constraint_pose.zbar_ij).inverse(); } - C_fixed_frames.emplace( - std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple(FromPose(fixed_frame_pose_in_map))); + C_fixed_frames.emplace(trajectory_id, + FromPose(fixed_frame_pose_in_map)); fixed_frame_pose_initialized = true; } - problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose), - nullptr /* loss function */, - C_fixed_frames.at(trajectory_id).data(), - C_nodes.at(node_id).data()); + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(constraint_pose), + options_.fixed_frame_pose_use_tolerant_loss() + ? new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) + : nullptr, + C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data()); } } From de76ed9fdc1b32e5080c1c4c87730e1097020135 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 9 Jul 2020 14:54:16 +0200 Subject: [PATCH 36/99] Add Debian Buster to the install script and CI. (#1715) Signed-off-by: Wolfgang Hess --- .travis.yml | 1 + Dockerfile.bionic | 1 - Dockerfile.buster | 32 ++++++++++++++++++++++++++++++++ Dockerfile.focal | 1 - Dockerfile.xenial | 1 - scripts/install_debs_cmake.sh | 22 +++++++++------------- 6 files changed, 42 insertions(+), 16 deletions(-) create mode 100644 Dockerfile.buster diff --git a/.travis.yml b/.travis.yml index f6a4e96299..987d35c762 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,6 +26,7 @@ env: - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ + - LSB_RELEASE=buster DOCKER_CACHE_FILE=/home/travis/docker/buster-cache.tar.gz CC=gcc CXX=g++ before_install: scripts/load_docker_cache.sh diff --git a/Dockerfile.bionic b/Dockerfile.bionic index e65606da99..7066e6aaca 100644 --- a/Dockerfile.bionic +++ b/Dockerfile.bionic @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - FROM ubuntu:bionic ARG cc diff --git a/Dockerfile.buster b/Dockerfile.buster new file mode 100644 index 0000000000..f587e9ee89 --- /dev/null +++ b/Dockerfile.buster @@ -0,0 +1,32 @@ +# Copyright 2020 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. + +FROM debian:buster + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.focal b/Dockerfile.focal index a2f7aed278..4abbe56504 100644 --- a/Dockerfile.focal +++ b/Dockerfile.focal @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - FROM ubuntu:focal ARG cc diff --git a/Dockerfile.xenial b/Dockerfile.xenial index 43231d5a0d..83620f90cf 100644 --- a/Dockerfile.xenial +++ b/Dockerfile.xenial @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - FROM ubuntu:xenial ARG cc diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index aed400f643..7eda82a3ac 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -37,21 +37,17 @@ sudo apt-get install -y \ ninja-build \ stow -if [[ "$(lsb_release -sc)" = "focal" ]] +# Install Ceres Solver and Protocol Buffers support if available. +# No need to build it ourselves. +if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] then - sudo apt-get install -y python3-sphinx libgmock-dev + sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler else - sudo apt-get install -y python-sphinx + sudo apt-get install -y python-sphinx + if [[ "$(lsb_release -sc)" = "bionic" ]] + then + sudo apt-get install -y libceres-dev + fi fi -# Install Ceres Solver on Ubuntu Bionic. No need to build it ourselves. -if [[ "$(lsb_release -sc)" = "bionic" ]] -then - sudo apt-get install -y libceres-dev -fi -# Install Ceres Solver and Protocol Buffers support on Ubuntu Focal. No need to build it ourselves. -if [[ "$(lsb_release -sc)" = "focal" ]] -then - sudo apt-get install -y libceres-dev protobuf-compiler -fi From 92f2a6714601cab004232ff45a447449ea2647b6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 10 Jul 2020 11:07:59 +0200 Subject: [PATCH 37/99] Remove unused code. (#1718) Signed-off-by: Wolfgang Hess --- cartographer/common/lockless_queue.h | 174 --------------------- cartographer/common/lockless_queue_test.cc | 26 --- 2 files changed, 200 deletions(-) delete mode 100644 cartographer/common/lockless_queue.h delete mode 100644 cartographer/common/lockless_queue_test.cc diff --git a/cartographer/common/lockless_queue.h b/cartographer/common/lockless_queue.h deleted file mode 100644 index 17c7ea0753..0000000000 --- a/cartographer/common/lockless_queue.h +++ /dev/null @@ -1,174 +0,0 @@ -/* - * 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_COMMON_LOCKLESS_QUEUE_H_ -#define CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ - -#include -#include - -#include "absl/memory/memory.h" -#include "glog/logging.h" - -namespace cartographer { -namespace common { - -// Lock-less queue which is thread safe for concurrent data producers and a -// single data consumer. -// -// This lockless queue implementation is adapted from -// https://github.com/resonance-audio/resonance-audio/blob/master/resonance_audio/utils/lockless_task_queue.h -template -class LocklessQueue { - public: - LocklessQueue() { - free_list_head_ = nullptr; - incoming_data_list_head_ = nullptr; - data_list_head_ = nullptr; - data_list_tail_ = nullptr; - } - - ~LocklessQueue() { - FreeNodes(free_list_head_.exchange(nullptr)); - FreeNodes(incoming_data_list_head_.exchange(nullptr)); - FreeNodes(data_list_head_); - } - - // Pushes the data item into the queue. - void Push(std::unique_ptr t) { - Node* const free_node = PopNodeFromFreeList(); - CHECK(free_node); - free_node->data = std::move(t); - PushNodeToList(&incoming_data_list_head_, free_node); - } - - // Pops the oldest data item from the queue. If the queue is empty returns a - // nullptr. - std::unique_ptr Pop() { - SwapLists(); - if (data_list_head_ != nullptr) { - Node* node = data_list_head_; - data_list_head_ = data_list_head_->next; - std::unique_ptr data = std::move(node->data); - PushNodeToList(&free_list_head_, node); - return data; - } - return nullptr; - } - - private: - // Node to model a single-linked list. - struct Node { - Node() = default; - - // Dummy copy constructor to enable vector::resize allocation. - Node(const Node& node) : next() {} - - // User data. - std::unique_ptr data; - - // Pointer to next node. - std::atomic next; - }; - - // Deallocates all nodes of the list starting with 'head'. - void FreeNodes(Node* node) { - while (node != nullptr) { - Node* next_node_ptr = node->next; - delete node; - node = next_node_ptr; - } - } - - // Pushes a node to the front of a list. - void PushNodeToList(std::atomic* list_head, Node* node) { - DCHECK(list_head); - DCHECK(node); - Node* list_head_ptr; - do { - list_head_ptr = list_head->load(); - node->next = list_head_ptr; - } while (!std::atomic_compare_exchange_strong_explicit( - list_head, &list_head_ptr, node, std::memory_order_release, - std::memory_order_relaxed)); - } - - // Pops a node from the front of the free node list. If the list is empty - // constructs a new node instance. - Node* PopNodeFromFreeList() { - Node* list_head_ptr; - Node* list_head_next_ptr; - do { - list_head_ptr = free_list_head_.load(); - if (list_head_ptr == nullptr) { - return new Node; - } - list_head_next_ptr = list_head_ptr->next.load(); - } while (!std::atomic_compare_exchange_strong_explicit( - &free_list_head_, &list_head_ptr, list_head_next_ptr, - std::memory_order_relaxed, std::memory_order_relaxed)); - return list_head_ptr; - } - - // Swaps the incoming data list for an empty list and appends all items - // to 'data_list_tail_'. - void SwapLists() { - Node* node_itr = incoming_data_list_head_.exchange(nullptr); - if (node_itr == nullptr) { - // There is no data on the incoming list. - return; - } - // The first node of the incoming data list will become the tail of the - // data list. - Node* const data_list_tail = node_itr; - - // Reverses the list order. After this operation 'prev_node_itr' points to - // head of the new data list items. - Node* prev_node_itr = nullptr; - while (node_itr != nullptr) { - Node* const next_node_ptr = node_itr->next; - node_itr->next = prev_node_itr; - prev_node_itr = node_itr; - node_itr = next_node_ptr; - } - - // If the previous data list was empty, replace head rather than appending - // to the list. - if (data_list_tail_ == nullptr) { - data_list_head_ = prev_node_itr; - } else { - data_list_tail_->next = prev_node_itr; - } - data_list_tail_ = data_list_tail; - } - - // Pointer to head node of free list. - std::atomic free_list_head_; - - // Pointer to head node of incoming data list, which is in FILO order. - std::atomic incoming_data_list_head_; - - // Pointer to head node of data list. - Node* data_list_head_; - - // Pointer to tail node of data list. - Node* data_list_tail_; -}; - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ diff --git a/cartographer/common/lockless_queue_test.cc b/cartographer/common/lockless_queue_test.cc deleted file mode 100644 index 5553c35ebf..0000000000 --- a/cartographer/common/lockless_queue_test.cc +++ /dev/null @@ -1,26 +0,0 @@ -#include "cartographer/common/lockless_queue.h" - -#include "gtest/gtest.h" - -namespace cartographer { -namespace common { -namespace { - -TEST(LocklessQueueTest, PushAndPop) { - LocklessQueue queue; - queue.Push(absl::make_unique(1)); - queue.Push(absl::make_unique(2)); - EXPECT_EQ(*queue.Pop(), 1); - queue.Push(absl::make_unique(3)); - queue.Push(absl::make_unique(4)); - EXPECT_EQ(*queue.Pop(), 2); - queue.Push(absl::make_unique(5)); - EXPECT_EQ(*queue.Pop(), 3); - EXPECT_EQ(*queue.Pop(), 4); - EXPECT_EQ(*queue.Pop(), 5); - EXPECT_EQ(queue.Pop(), nullptr); -} - -} // namespace -} // namespace common -} // namespace cartographer \ No newline at end of file From a62fb739af83865355cb92563897bc3410917fcd Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 23 Jul 2020 10:12:11 +0200 Subject: [PATCH 38/99] Update package.xml for ROS Noetic. (#1723) Signed-off-by: Wolfgang Hess --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 691be44ab5..a8819cbde6 100644 --- a/package.xml +++ b/package.xml @@ -15,7 +15,7 @@ limitations under the License. --> - + cartographer 1.0.0 @@ -38,7 +38,7 @@ git google-mock - python-sphinx + python3-sphinx boost ceres-solver From 3611b52e908470cf31e3ceb9589440ec5cf77fb4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 23 Jul 2020 11:46:59 +0200 Subject: [PATCH 39/99] Only set C++11 explicitly for old versions of GCC. (#1724) Starting with GCC 6.1, C++14 became the default. Signed-off-by: Wolfgang Hess --- cmake/functions.cmake | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/cmake/functions.cmake b/cmake/functions.cmake index a9336aa535..1bbce430c2 100644 --- a/cmake/functions.cmake +++ b/cmake/functions.cmake @@ -76,7 +76,11 @@ macro(google_initialize_cartographer_project) if(WIN32) # TODO turn on equivalent warnings on Windows else() - set(GOOG_CXX_FLAGS "-pthread -std=c++11 -fPIC ${GOOG_CXX_FLAGS}") + set(GOOG_CXX_FLAGS "-pthread -fPIC ${GOOG_CXX_FLAGS}") + + if (CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 6.1) + google_add_flag(GOOG_CXX_FLAGS "-std=c++11") + endif() google_add_flag(GOOG_CXX_FLAGS "-Wall") google_add_flag(GOOG_CXX_FLAGS "-Wpedantic") From 340a9ac21a5a7f77febcbec3d3ffa312b5c3063e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Tue, 28 Jul 2020 22:53:12 +0200 Subject: [PATCH 40/99] add pose extrapolator interface and extrapolator options (#1726) This adds an interface around the extrapolator without further integration into any call sides. Currently there is only a constant-velocity extrapolator implementation. The next step is to add an extrapolator implementation which integrates imu measurements for pose extrapolation. Signed-off-by: mschworer --- cartographer/mapping/pose_extrapolator.cc | 16 ++++ cartographer/mapping/pose_extrapolator.h | 20 +++-- .../mapping/pose_extrapolator_interface.cc | 61 ++++++++++++++ .../mapping/pose_extrapolator_interface.h | 80 +++++++++++++++++++ .../proto/pose_extrapolator_options.proto | 32 ++++++++ 5 files changed, 201 insertions(+), 8 deletions(-) create mode 100644 cartographer/mapping/pose_extrapolator_interface.cc create mode 100644 cartographer/mapping/pose_extrapolator_interface.h create mode 100644 cartographer/mapping/proto/pose_extrapolator_options.proto diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index d02fd5b16b..24e3800887 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -242,5 +242,21 @@ Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { return extrapolation_delta * linear_velocity_from_odometry_; } +PoseExtrapolator::ExtrapolationResult +PoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + std::vector poses; + for (auto it = times.begin(); it != std::prev(times.end()); ++it) { + poses.push_back(ExtrapolatePose(*it).cast()); + } + + const Eigen::Vector3d current_velocity = odometry_data_.size() < 2 + ? linear_velocity_from_poses_ + : linear_velocity_from_odometry_; + return ExtrapolationResult{poses, ExtrapolatePose(times.back()), + current_velocity, + EstimateGravityOrientation(times.back())}; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 0b14c12af2..1c6885d994 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -22,6 +22,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/imu_tracker.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" @@ -32,7 +33,7 @@ namespace mapping { // Keep poses for a certain duration to estimate linear and angular velocity. // Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if // available to improve the extrapolation. -class PoseExtrapolator { +class PoseExtrapolator : public PoseExtrapolatorInterface { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); @@ -46,17 +47,20 @@ class PoseExtrapolator { // Returns the time of the last added pose or Time::min() if no pose was added // yet. - common::Time GetLastPoseTime() const; - common::Time GetLastExtrapolatedTime() const; + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; - void AddPose(common::Time time, const transform::Rigid3d& pose); - void AddImuData(const sensor::ImuData& imu_data); - void AddOdometryData(const sensor::OdometryData& odometry_data); - transform::Rigid3d ExtrapolatePose(common::Time time); + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; // Returns the current gravity alignment estimate as a rotation from // the tracking frame into a gravity aligned frame. - Eigen::Quaterniond EstimateGravityOrientation(common::Time time); + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; private: void UpdateVelocitiesFromPoses(); diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc new file mode 100644 index 0000000000..a563e7b829 --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -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. + */ + +#include "cartographer/mapping/pose_extrapolator_interface.h" + +#include "cartographer/common/time.h" +#include "cartographer/mapping/pose_extrapolator.h" + +namespace cartographer { +namespace mapping { + +namespace { + +proto::ConstantVelocityPoseExtrapolatorOptions +CreateConstantVelocityPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ConstantVelocityPoseExtrapolatorOptions options; + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + return options; +} + +} + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::PoseExtrapolatorOptions options; + *options.mutable_constant_velocity() = + CreateConstantVelocityPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("constant_velocity").get()); + return options; +} + +std::unique_ptr +PoseExtrapolatorInterface::CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data) { + CHECK(!imu_data.empty()); + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); +} + +} +} diff --git a/cartographer/mapping/pose_extrapolator_interface.h b/cartographer/mapping/pose_extrapolator_interface.h new file mode 100644 index 0000000000..6f470db6fc --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.h @@ -0,0 +1,80 @@ +/* + * 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_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class PoseExtrapolatorInterface { + public: + struct ExtrapolationResult { + // The poses for the requested times at index 0 to N-1. + std::vector previous_poses; + // The pose for the requested time at index N. + transform::Rigid3d current_pose; + Eigen::Vector3d current_velocity; + Eigen::Quaterniond gravity_from_tracking; + }; + + PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; + PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete; + virtual ~PoseExtrapolatorInterface() {} + + // TODO: Remove dependency cycle. + static std::unique_ptr CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + virtual common::Time GetLastPoseTime() const = 0; + virtual common::Time GetLastExtrapolatedTime() const = 0; + + virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0; + virtual void AddImuData(const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0; + virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0; + + virtual ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) = 0; + + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. + virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0; + + protected: + PoseExtrapolatorInterface() {} +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ diff --git a/cartographer/mapping/proto/pose_extrapolator_options.proto b/cartographer/mapping/proto/pose_extrapolator_options.proto new file mode 100644 index 0000000000..ede5fd84cd --- /dev/null +++ b/cartographer/mapping/proto/pose_extrapolator_options.proto @@ -0,0 +1,32 @@ +// 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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message ConstantVelocityPoseExtrapolatorOptions { + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + double imu_gravity_time_constant = 1; + double pose_queue_duration = 2; +} + +message PoseExtrapolatorOptions { + ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2; +} From 1cae11593d813df7be11e45dc3722caadff2899f Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 29 Jul 2020 11:53:14 +0200 Subject: [PATCH 41/99] Add gauge metric for pose graph work queue size. (#1727) Signed-off-by: Michael Grupp --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 7 +++++++ cartographer/mapping/internal/3d/pose_graph_3d.cc | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 6ccf43bc6f..fe85dce000 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -42,6 +42,7 @@ namespace cartographer { namespace mapping { static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); @@ -181,6 +182,7 @@ void PoseGraph2D::AddWorkItem( } const auto now = std::chrono::steady_clock::now(); work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); kWorkQueueDelayMetric->Set( std::chrono::duration_cast>( now - work_queue_->front().time) @@ -529,6 +531,7 @@ void PoseGraph2D::DrainWorkQueue() { work_item = work_queue_->front().task; work_queue_->pop_front(); work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); } process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; } @@ -1288,6 +1291,10 @@ void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) { "mapping_2d_pose_graph_work_queue_delay", "Age of the oldest entry in the work queue in seconds"); kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); auto* constraints = family_factory->NewGaugeFamily( "mapping_2d_pose_graph_constraints", "Current number of constraints in the pose graph"); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index cd470733ec..d6410f7347 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -40,6 +40,7 @@ namespace cartographer { namespace mapping { static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); @@ -169,6 +170,7 @@ void PoseGraph3D::AddWorkItem( } const auto now = std::chrono::steady_clock::now(); work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); kWorkQueueDelayMetric->Set( std::chrono::duration_cast>( now - work_queue_->front().time) @@ -516,6 +518,7 @@ void PoseGraph3D::DrainWorkQueue() { work_item = work_queue_->front().task; work_queue_->pop_front(); work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); } process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; } @@ -1268,6 +1271,10 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) { "mapping_3d_pose_graph_work_queue_delay", "Age of the oldest entry in the work queue in seconds"); kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_3d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); auto* constraints = family_factory->NewGaugeFamily( "mapping_3d_pose_graph_constraints", "Current number of constraints in the pose graph"); From 32dad63edd74e8e951b8e181bae6acc244367e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 29 Jul 2020 14:08:40 +0200 Subject: [PATCH 42/99] extracted eigen quaterniond from 2 vectors for faster compilation (#1729) Signed-off-by: mschworer --- .../eigen_quaterniond_from_two_vectors.cc | 28 +++++++++++++++ .../eigen_quaterniond_from_two_vectors.h | 34 +++++++++++++++++++ cartographer/mapping/imu_tracker.cc | 3 +- cartographer/mapping/imu_tracker_test.cc | 6 ++-- .../mapping/pose_extrapolator_test.cc | 10 +++--- 5 files changed, 72 insertions(+), 9 deletions(-) create mode 100644 cartographer/mapping/eigen_quaterniond_from_two_vectors.cc create mode 100644 cartographer/mapping/eigen_quaterniond_from_two_vectors.h diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc b/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc new file mode 100644 index 0000000000..af0aac7f94 --- /dev/null +++ b/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc @@ -0,0 +1,28 @@ +/* + * 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/mapping/eigen_quaterniond_from_two_vectors.h" + +namespace cartographer { +namespace mapping { + +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b) { + return Eigen::Quaterniond::FromTwoVectors(a, b); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.h b/cartographer/mapping/eigen_quaterniond_from_two_vectors.h new file mode 100644 index 0000000000..e8d04b370d --- /dev/null +++ b/cartographer/mapping/eigen_quaterniond_from_two_vectors.h @@ -0,0 +1,34 @@ +/* + * 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_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ +#define CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ + +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping { + +// Calls Eigen::Quaterniond::FromTwoVectors(). This is in its own compilation +// unit since it can take more than 10 s to build while using more than 1 GB of +// memory causing slow build times and high peak memory usage. +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc index 39cac08fcb..45391d00bf 100644 --- a/cartographer/mapping/imu_tracker.cc +++ b/cartographer/mapping/imu_tracker.cc @@ -20,6 +20,7 @@ #include #include "cartographer/common/math.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -60,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation( (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; // Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. - const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( + const Eigen::Quaterniond rotation = FromTwoVectors( gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized(); CHECK_GT((orientation_ * gravity_vector_).z(), 0.); diff --git a/cartographer/mapping/imu_tracker_test.cc b/cartographer/mapping/imu_tracker_test.cc index 8f3b452bf7..88ae846221 100644 --- a/cartographer/mapping/imu_tracker_test.cc +++ b/cartographer/mapping/imu_tracker_test.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/imu_tracker.h" #include "absl/memory/memory.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "gtest/gtest.h" namespace cartographer { @@ -82,9 +83,8 @@ TEST_F(ImuTrackerTest, IntegrateFullRotation) { TEST_F(ImuTrackerTest, LearnGravityVector) { linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5); AdvanceImu(); - Eigen::Quaterniond expected_orientation; - expected_orientation.setFromTwoVectors(linear_acceleration_, - Eigen::Vector3d::UnitZ()); + const Eigen::Quaterniond expected_orientation = + FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., imu_tracker_->orientation().angularDistance(expected_orientation), kPrecision); diff --git a/cartographer/mapping/pose_extrapolator_test.cc b/cartographer/mapping/pose_extrapolator_test.cc index fe17e1bc22..38afc84a9c 100644 --- a/cartographer/mapping/pose_extrapolator_test.cc +++ b/cartographer/mapping/pose_extrapolator_test.cc @@ -18,6 +18,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" @@ -71,9 +72,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { angular_velocity}; auto extrapolator = PoseExtrapolator::InitializeWithImu( common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data); - Eigen::Quaterniond expected_orientation; - expected_orientation.setFromTwoVectors(initial_gravity_acceleration, - Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond expected_orientation = + FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation), @@ -84,8 +84,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { extrapolator->AddImuData( sensor::ImuData{current_time, gravity_acceleration, angular_velocity}); } - expected_orientation.setFromTwoVectors(gravity_acceleration, - Eigen::Vector3d::UnitZ()); + expected_orientation = + FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation), From 23351be571e96417366f806b5cdac9e892bc2661 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 29 Jul 2020 14:48:01 +0200 Subject: [PATCH 43/99] added imu based extrapolator impl (#1728) This adds an alternative implementation for pose extrapolation which uses imu integration as well as past local SLAM results (and odometry data) to predict a pose. This allows to perform motion compensation and initialize scan matching afterwards. Using imu data for pose extrapolation and motion compensation is more accurate, because it does not make a constant velocity assumption like the PoseExtrapolator implementation. Next steps are the integration into local trajectory builder as well as tuning the parameters for existing datasets. This PR is quite large but was already reviewed in smaller chunks internally and combines contributions from @danielsievers, @wohe and @schwoere. Signed-off-by: mschworer --- .../mapping/imu_based_pose_extrapolator.cc | 439 ++++++++++++++++++ .../mapping/imu_based_pose_extrapolator.h | 102 ++++ .../mapping/internal/3d/imu_integration.h | 65 +++ .../proto/pose_extrapolator_options.proto | 16 + 4 files changed, 622 insertions(+) create mode 100644 cartographer/mapping/imu_based_pose_extrapolator.cc create mode 100644 cartographer/mapping/imu_based_pose_extrapolator.h diff --git a/cartographer/mapping/imu_based_pose_extrapolator.cc b/cartographer/mapping/imu_based_pose_extrapolator.cc new file mode 100644 index 0000000000..6c9d72f27b --- /dev/null +++ b/cartographer/mapping/imu_based_pose_extrapolator.cc @@ -0,0 +1,439 @@ +/* + * 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/mapping/imu_based_pose_extrapolator.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/3d/imu_integration.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +using ::cartographer::transform::TimestampedTransform; + +ImuBasedPoseExtrapolator::ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options) + : options_(options), + solver_options_( + common::CreateCeresSolverOptions(options_.solver_options())) {} + +ImuBasedPoseExtrapolator::~ImuBasedPoseExtrapolator() { + LOG(INFO) << "Number of iterations for pose extrapolation:"; + LOG(INFO) << num_iterations_hist_.ToString(10); +} + +std::unique_ptr +ImuBasedPoseExtrapolator::InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses) { + CHECK(!imu_data.empty()); + LOG(INFO) << options.DebugString(); + auto extrapolator = absl::make_unique(options); + std::copy(imu_data.begin(), imu_data.end(), + std::back_inserter(extrapolator->imu_data_)); + if (!initial_poses.empty()) { + for (const auto& pose : initial_poses) { + if (pose.time > imu_data.front().time) { + extrapolator->AddPose(pose.time, pose.transform); + } + } + } else { + extrapolator->AddPose( + imu_data.back().time, + transform::Rigid3d::Rotation(FromTwoVectors( + imu_data.back().linear_acceleration, Eigen::Vector3d::UnitZ()))); + } + return extrapolator; +} + +common::Time ImuBasedPoseExtrapolator::GetLastPoseTime() const { + if (timed_pose_queue_.empty()) { + return common::Time::min(); + } + return timed_pose_queue_.back().time; +} + +common::Time ImuBasedPoseExtrapolator::GetLastExtrapolatedTime() const { + return last_extrapolated_time_; +} + +void ImuBasedPoseExtrapolator::AddPose(const common::Time time, + const transform::Rigid3d& pose) { + timed_pose_queue_.push_back(TimestampedTransform{time, pose}); + while (timed_pose_queue_.size() > 3 && + timed_pose_queue_[1].time <= + time - common::FromSeconds(options_.pose_queue_duration())) { + if (!previous_solution_.empty()) { + CHECK_EQ(timed_pose_queue_.front().time, previous_solution_.front().time); + previous_solution_.pop_front(); + } + timed_pose_queue_.pop_front(); + } + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { + CHECK(timed_pose_queue_.empty() || + imu_data.time >= timed_pose_queue_.back().time); + imu_data_.push_back(imu_data); + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddOdometryData( + const sensor::OdometryData& odometry_data) { + CHECK(timed_pose_queue_.empty() || + odometry_data.time >= timed_pose_queue_.back().time); + odometry_data_.push_back(odometry_data); + TrimOdometryData(); +} + +ImuBasedPoseExtrapolator::ExtrapolationResult +ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + const auto& time = times.back(); + const auto& newest_timed_pose = timed_pose_queue_.back(); + CHECK_GE(time, newest_timed_pose.time); + CHECK_GE(times.size(), 1); + last_extrapolated_time_ = time; + + if (timed_pose_queue_.size() < 3 || + common::ToSeconds(time - newest_timed_pose.time) < 1e-6) { + return ExtrapolationResult{ + std::vector( + times.size() - 1, timed_pose_queue_.back().transform.cast()), + timed_pose_queue_.back().transform, Eigen::Vector3d::Zero(), + timed_pose_queue_.back().transform.rotation()}; + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Track gravity alignment over time and use this as a frame here so that + // we can estimate the gravity alignment of the current pose. + optimization::CeresPose gravity_from_local( + gravity_from_local_, nullptr, + absl::make_unique(), &problem); + // Use deque so addresses stay constant during problem formulation. + std::deque nodes; + std::vector node_times; + + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const bool is_last = (i == (timed_pose_queue_.size() - 1)); + const auto& timed_pose = timed_pose_queue_[i]; + node_times.push_back(timed_pose.time); + + transform::Rigid3d gravity_from_node; + // Use the last scan match result (timed_pose_queue_.back()) for + // initialization here instead of the last result from the optimization. + // This keeps poses from slowly drifting apart due to lack of feedback + // from the scan matching here. + if (!previous_solution_.empty() && !is_last) { + CHECK_GT(previous_solution_.size(), i); + CHECK_EQ(timed_pose.time, previous_solution_[i].time); + gravity_from_node = previous_solution_[i].transform; + } else { + gravity_from_node = gravity_from_local_ * timed_pose.transform; + } + + if (is_last) { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique>(), + &problem); + problem.SetParameterBlockConstant(nodes.back().translation()); + } else { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique(), + &problem); + } + } + + double gravity_constant = 9.8; + bool fix_gravity = false; + if (options_.gravity_constant() > 0) { + fix_gravity = true; + gravity_constant = options_.gravity_constant(); + } + + auto imu_it_prev_prev = imu_data_.begin(); + while (imu_it_prev_prev != std::prev(imu_data_.end()) && + std::next(imu_it_prev_prev)->time <= timed_pose_queue_.back().time) { + ++imu_it_prev_prev; + } + + const TimestampedTransform prev_gravity_from_tracking = + TimestampedTransform{node_times.back(), nodes.back().ToRigid()}; + const TimestampedTransform prev_prev_gravity_from_tracking = + TimestampedTransform{node_times.at(node_times.size() - 2), + nodes.at(nodes.size() - 2).ToRigid()}; + const transform::Rigid3d initial_estimate = + ExtrapolatePoseWithImu( + prev_gravity_from_tracking.transform, prev_gravity_from_tracking.time, + prev_prev_gravity_from_tracking.transform, + prev_prev_gravity_from_tracking.time, + gravity_constant * Eigen::Vector3d::UnitZ(), time, imu_data_, + &imu_it_prev_prev) + .pose; + nodes.emplace_back(initial_estimate, nullptr, + absl::make_unique(), + &problem); + node_times.push_back(time); + + // Add cost functions for node constraints. + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const auto& timed_pose = timed_pose_queue_[i]; + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + timed_pose.transform, options_.pose_translation_weight(), + options_.pose_rotation_weight()}), + nullptr /* loss function */, gravity_from_local.rotation(), + gravity_from_local.translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + } + + CHECK(!imu_data_.empty()); + CHECK_LE(imu_data_.front().time, timed_pose_queue_.front().time); + + std::array imu_calibration{{1., 0., 0., 0.}}; + + problem.AddParameterBlock(imu_calibration.data(), 4, + new ceres::QuaternionParameterization()); + problem.SetParameterBlockConstant(imu_calibration.data()); + + auto imu_it = imu_data_.begin(); + CHECK(imu_data_.size() == 1 || + std::next(imu_it)->time > timed_pose_queue_.front().time); + + transform::Rigid3d last_node_odometry; + common::Time last_node_odometry_time; + + for (size_t i = 1; i < nodes.size(); i++) { + const common::Time first_time = node_times[i - 1]; + const common::Time second_time = node_times[i]; + + auto imu_it2 = imu_it; + const IntegrateImuResult result = + IntegrateImu(imu_data_, first_time, second_time, &imu_it); + if ((i + 1) < nodes.size()) { + const common::Time third_time = node_times[i + 1]; + const common::Duration first_duration = second_time - first_time; + const common::Duration second_duration = third_time - second_time; + const common::Time first_center = first_time + first_duration / 2; + const common::Time second_center = second_time + second_duration / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data_, first_time, first_center, &imu_it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data_, first_center, second_center, &imu_it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between + // second and third pose. It is computed from IMU data and still + // contains a delta due to gravity. The orientation of this vector is + // in the IMU frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + AccelerationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_acceleration_weight(), delta_velocity, + common::ToSeconds(first_duration), + common::ToSeconds(second_duration)), + nullptr /* loss function */, nodes.at(i).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).translation(), + nodes.at(i + 1).translation(), &gravity_constant, + imu_calibration.data()); + // TODO(danielsievers): Fix gravity in CostFunction. + if (fix_gravity) { + problem.SetParameterBlockConstant(&gravity_constant); + } else { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&gravity_constant, 0, 0.0); + } + } + problem.AddResidualBlock( + RotationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_rotation_weight(), result.delta_rotation), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i).rotation(), imu_calibration.data()); + + // Add a relative pose constraint based on the odometry (if available). + if (HasOdometryDataForTime(first_time) && + HasOdometryDataForTime(second_time)) { + // Here keep track of last node odometry to avoid double computation. + // Do this if first loop, or if there were some missing odometry nodes + // then recalculate. + if (i == 1 || last_node_odometry_time != first_time) { + last_node_odometry = InterpolateOdometry(first_time); + last_node_odometry_time = first_time; + } + const transform::Rigid3d current_node_odometry = + InterpolateOdometry(second_time); + const transform::Rigid3d relative_odometry = + CalculateOdometryBetweenNodes(last_node_odometry, + current_node_odometry); + + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + // Use the current node odometry in the next iteration + last_node_odometry = current_node_odometry; + last_node_odometry_time = second_time; + } + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options_, &problem, &summary); + LOG_IF_EVERY_N(INFO, !fix_gravity, 20) << "Gravity was: " << gravity_constant; + + const auto gravity_estimate = nodes.back().ToRigid().rotation(); + + const auto& last_pose = timed_pose_queue_.back(); + const auto extrapolated_pose = TimestampedTransform{ + time, last_pose.transform * + nodes.at(nodes.size() - 2).ToRigid().inverse() * + nodes.back().ToRigid()}; + + num_iterations_hist_.Add(summary.iterations.size()); + + gravity_from_local_ = gravity_from_local.ToRigid(); + + previous_solution_.clear(); + for (size_t i = 0; i < nodes.size(); ++i) { + previous_solution_.push_back( + TimestampedTransform{node_times.at(i), nodes.at(i).ToRigid()}); + } + + const Eigen::Vector3d current_velocity = + (extrapolated_pose.transform.translation() - + last_pose.transform.translation()) / + common::ToSeconds(time - last_pose.time); + + return ExtrapolationResult{ + InterpolatePoses(last_pose, extrapolated_pose, times.begin(), + std::prev(times.end())), + extrapolated_pose.transform, current_velocity, gravity_estimate}; +} + +std::vector ImuBasedPoseExtrapolator::InterpolatePoses( + const TimestampedTransform& start, const TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end) { + std::vector poses; + poses.reserve(std::distance(times_begin, times_end)); + const float duration_scale = 1. / common::ToSeconds(end.time - start.time); + + const Eigen::Quaternionf start_rotation = + Eigen::Quaternionf(start.transform.rotation()); + const Eigen::Quaternionf end_rotation = + Eigen::Quaternionf(end.transform.rotation()); + const Eigen::Vector3f start_translation = + start.transform.translation().cast(); + const Eigen::Vector3f end_translation = + end.transform.translation().cast(); + + for (auto it = times_begin; it != times_end; ++it) { + const float factor = common::ToSeconds(*it - start.time) * duration_scale; + const Eigen::Vector3f origin = + start_translation + (end_translation - start_translation) * factor; + const Eigen::Quaternionf rotation = + start_rotation.slerp(factor, end_rotation); + poses.emplace_back(origin, rotation); + } + return poses; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::ExtrapolatePose( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .current_pose; +} + +Eigen::Quaterniond ImuBasedPoseExtrapolator::EstimateGravityOrientation( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .gravity_from_tracking; +} + +template +void ImuBasedPoseExtrapolator::TrimDequeData(std::deque* data) { + while (data->size() > 1 && !timed_pose_queue_.empty() && + data->at(1).time <= timed_pose_queue_.front().time) { + data->pop_front(); + } +} + +void ImuBasedPoseExtrapolator::TrimImuData() { + TrimDequeData(&imu_data_); +} + +void ImuBasedPoseExtrapolator::TrimOdometryData() { + TrimDequeData(&odometry_data_); +} + +// Odometry methods +bool ImuBasedPoseExtrapolator::HasOdometryData() const { + return odometry_data_.size() >= 2; +} + +bool ImuBasedPoseExtrapolator::HasOdometryDataForTime( + const common::Time& time) const { + return HasOdometryData() && odometry_data_.front().time < time && + time < odometry_data_.back().time; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::InterpolateOdometry( + const common::Time& time) const { + // Only interpolate if time is within odometry data range. + CHECK(HasOdometryDataForTime(time)) + << "Odometry data range does not include time " << time; + std::deque::const_iterator data = std::upper_bound( + odometry_data_.begin(), odometry_data_.end(), time, + [](const common::Time& time, const sensor::OdometryData& odometry_data) { + return time < odometry_data.time; + }); + const TimestampedTransform first{std::prev(data)->time, + std::prev(data)->pose}; + const TimestampedTransform second{data->time, data->pose}; + return Interpolate(first, second, time).transform; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const { + return first_node_odometry.inverse() * second_node_odometry; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/imu_based_pose_extrapolator.h b/cartographer/mapping/imu_based_pose_extrapolator.h new file mode 100644 index 0000000000..1234fd00bd --- /dev/null +++ b/cartographer/mapping/imu_based_pose_extrapolator.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ +#define CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ + +#include +#include +#include + +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/histogram.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +// Uses the linear acceleration and rotational velocities to estimate a pose. +class ImuBasedPoseExtrapolator : public PoseExtrapolatorInterface { + public: + explicit ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options); + ~ImuBasedPoseExtrapolator() override; + + static std::unique_ptr InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; + + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; + + // Gravity alignment estimate. + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; + + private: + template + void TrimDequeData(std::deque* data); + + void TrimImuData(); + void TrimOdometryData(); + + // Odometry methods. + bool HasOdometryData() const; + bool HasOdometryDataForTime(const common::Time& first_time) const; + transform::Rigid3d InterpolateOdometry(const common::Time& first_time) const; + transform::Rigid3d CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const; + + std::vector InterpolatePoses( + const ::cartographer::transform::TimestampedTransform& start, + const ::cartographer::transform::TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end); + + std::deque<::cartographer::transform::TimestampedTransform> timed_pose_queue_; + std::deque<::cartographer::transform::TimestampedTransform> + previous_solution_; + + std::deque imu_data_; + std::deque odometry_data_; + common::Time last_extrapolated_time_ = common::Time::min(); + + transform::Rigid3d gravity_from_local_ = transform::Rigid3d::Identity(); + + const proto::ImuBasedPoseExtrapolatorOptions options_; + const ceres::Solver::Options solver_options_; + + common::Histogram num_iterations_hist_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ diff --git a/cartographer/mapping/internal/3d/imu_integration.h b/cartographer/mapping/internal/3d/imu_integration.h index 88ee221c6c..b89875c1de 100644 --- a/cartographer/mapping/internal/3d/imu_integration.h +++ b/cartographer/mapping/internal/3d/imu_integration.h @@ -24,6 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/time.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -33,6 +34,7 @@ namespace mapping { template struct IntegrateImuResult { Eigen::Matrix delta_velocity; + Eigen::Matrix delta_translation; Eigen::Quaternion delta_rotation; }; @@ -54,6 +56,7 @@ IntegrateImuResult IntegrateImu( common::Time current_time = start_time; IntegrateImuResult result = {Eigen::Matrix::Zero(), + Eigen::Matrix::Zero(), Eigen::Quaterniond::Identity().cast()}; while (current_time < end_time) { common::Time next_imu_data = common::Time::max(); @@ -73,6 +76,7 @@ IntegrateImuResult IntegrateImu( ((linear_acceleration_calibration * (*it)->linear_acceleration.template cast()) * delta_t); + result.delta_translation += delta_t * result.delta_velocity; current_time = next_time; if (current_time == next_imu_data) { ++(*it); @@ -92,6 +96,67 @@ IntegrateImuResult IntegrateImu(const RangeType& imu_data, start_time, end_time, it); } +template +struct ExtrapolatePoseResult { + transform::Rigid3 pose; + Eigen::Matrix velocity; +}; + +// Returns pose and linear velocity at 'time' which is equal to +// 'prev_from_tracking' extrapolated using IMU data. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const Eigen::Matrix& prev_velocity_in_tracking, + const common::Time prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + const IntegrateImuResult result = + IntegrateImu(imu_data, Eigen::Transform::Identity(), + Eigen::Transform::Identity(), prev_time, + time, imu_it); + + const T delta_t = static_cast(common::ToSeconds(time - prev_time)); + const Eigen::Matrix translation = + prev_from_tracking.translation() + + prev_from_tracking.rotation() * + (delta_t * prev_velocity_in_tracking + result.delta_translation) - + static_cast(.5) * delta_t * delta_t * gravity; + const Eigen::Quaternion rotation = + prev_from_tracking.rotation() * result.delta_rotation; + + const Eigen::Matrix velocity = + prev_from_tracking.rotation() * + (prev_velocity_in_tracking + result.delta_velocity) - + delta_t * gravity; + return ExtrapolatePoseResult{transform::Rigid3(translation, rotation), + velocity}; +} + +// Same as above but given the last two poses. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const common::Time prev_time, + const transform::Rigid3& prev_prev_from_tracking, + const common::Time prev_prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + // TODO(danielsievers): Really we should integrate velocities starting from + // the midpoint in between two poses, since this is how we fit them to poses + // in the optimization. + const T prev_delta_t = + static_cast(common::ToSeconds(prev_time - prev_prev_time)); + const Eigen::Matrix prev_velocity_in_tracking = + prev_from_tracking.inverse().rotation() * + (prev_from_tracking.translation() - + prev_prev_from_tracking.translation()) / + prev_delta_t; + + return ExtrapolatePoseWithImu(prev_from_tracking, prev_velocity_in_tracking, + prev_time, gravity, time, imu_data, imu_it); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/proto/pose_extrapolator_options.proto b/cartographer/mapping/proto/pose_extrapolator_options.proto index ede5fd84cd..ff6a4538d7 100644 --- a/cartographer/mapping/proto/pose_extrapolator_options.proto +++ b/cartographer/mapping/proto/pose_extrapolator_options.proto @@ -14,6 +14,8 @@ syntax = "proto3"; +import "cartographer/common/proto/ceres_solver_options.proto"; + package cartographer.mapping.proto; message ConstantVelocityPoseExtrapolatorOptions { @@ -27,6 +29,20 @@ message ConstantVelocityPoseExtrapolatorOptions { double pose_queue_duration = 2; } +message ImuBasedPoseExtrapolatorOptions { + double pose_queue_duration = 1; + double gravity_constant = 2; + double pose_translation_weight = 3; + double pose_rotation_weight = 4; + double imu_acceleration_weight = 5; + double imu_rotation_weight = 6; + cartographer.common.proto.CeresSolverOptions solver_options = 7; + double odometry_translation_weight = 8; + double odometry_rotation_weight = 9; +} + message PoseExtrapolatorOptions { + bool use_imu_based = 1; ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2; + ImuBasedPoseExtrapolatorOptions imu_based = 3; } From efc64934d640f3f7561693b09feb3c6b5b0e0fe7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 29 Jul 2020 19:15:15 +0200 Subject: [PATCH 44/99] use extrapolator interface (#1730) This integrates the extrapolator interface into the 3D trajectory builder. The construction is now done within the interface and local trajectory builder 3D just makes use of the interface. No functional changes. Signed-off-by: mschworer --- .../internal/2d/local_trajectory_builder_2d.cc | 13 +++++++------ .../2d/local_trajectory_builder_options_2d.cc | 3 +++ .../internal/3d/local_trajectory_builder_3d.cc | 9 ++------- .../internal/3d/local_trajectory_builder_3d.h | 4 ++-- .../3d/local_trajectory_builder_3d_test.cc | 9 ++++++++- .../3d/local_trajectory_builder_options_3d.cc | 3 +++ .../mapping/pose_extrapolator_interface.cc | 13 +++++++------ .../local_trajectory_builder_options_2d.proto | 15 ++++++++++----- .../local_trajectory_builder_options_3d.proto | 17 +++++++++++------ configuration_files/trajectory_builder_2d.lua | 7 +++++++ configuration_files/trajectory_builder_3d.lua | 10 +++++++++- 11 files changed, 69 insertions(+), 34 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 08823e01cd..55a1e4f800 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -319,14 +319,15 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { if (extrapolator_ != nullptr) { return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; + CHECK(!options_.pose_extrapolator_options().use_imu_based()); // TODO(gaschler): Consider using InitializeWithImu as 3D does. extrapolator_ = absl::make_unique( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant()); + ::cartographer::common::FromSeconds(options_.pose_extrapolator_options() + .constant_velocity() + .pose_queue_duration()), + options_.pose_extrapolator_options() + .constant_velocity() + .imu_gravity_time_constant()); extrapolator_->AddPose(time, transform::Rigid3d::Identity()); } diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc index 552e0d5893..434da3d0f3 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" namespace cartographer { @@ -58,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); *options.mutable_submaps_options() = CreateSubmapsOptions2D( diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 5cd26ab11d..1f670e2582 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -108,13 +108,8 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { extrapolator_->AddImuData(imu_data); return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; - extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant(), imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( + options_.pose_extrapolator_options(), {imu_data}); } std::unique_ptr diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 2abeee857e..b0ee05bc7d 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -26,7 +26,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" -#include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" @@ -103,7 +103,7 @@ class LocalTrajectoryBuilder3D { real_time_correlative_scan_matcher_; std::unique_ptr ceres_scan_matcher_; - std::unique_ptr extrapolator_; + std::unique_ptr extrapolator_; int num_accumulated_ = 0; sensor::RangeData accumulated_range_data_; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 423ee91207..0558b0d053 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -96,7 +96,14 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { imu_gravity_time_constant = 1., rotational_histogram_size = 120, - + + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, + submaps = { high_resolution = 0.2, high_resolution_max_range = 50., diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc index 92c9ed1cc7..6dd63abdeb 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" #include "glog/logging.h" @@ -57,6 +58,8 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_rotational_histogram_size( diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index a563e7b829..32a5abd884 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -35,7 +35,7 @@ CreateConstantVelocityPoseExtrapolatorOptions( return options; } -} +} // namespace proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( common::LuaParameterDictionary* const parameter_dictionary) { @@ -51,11 +51,12 @@ PoseExtrapolatorInterface::CreateWithImuData( const proto::PoseExtrapolatorOptions& options, const std::vector& imu_data) { CHECK(!imu_data.empty()); + // TODO(schwoere): Implement/integrate imu based extrapolator. + CHECK(!options.use_imu_based()) << "Not implemented!"; return PoseExtrapolator::InitializeWithImu( - common::FromSeconds(options.constant_velocity().pose_queue_duration()), - options.constant_velocity().imu_gravity_time_constant(), - imu_data.back()); + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), imu_data.back()); } -} -} +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto index 21890916ae..0005c513f1 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto @@ -17,11 +17,13 @@ syntax = "proto3"; package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping/proto/2d/submaps_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +// NEXT ID: 22 message LocalTrajectoryBuilderOptions2D { // Rangefinder points outside these ranges will be dropped. float min_range = 14; @@ -58,12 +60,15 @@ message LocalTrajectoryBuilderOptions2D { MotionFilterOptions motion_filter_options = 13; // Time constant in seconds for the orientation moving average based on - // observed gravity via the IMU. It should be chosen so that the error - // 1. from acceleration measurements not due to gravity (which gets worse when - // the constant is reduced) and - // 2. from integration of angular velocities (which gets worse when the - // constant is increased) is balanced. + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; SubmapsOptions2D submaps_options = 11; diff --git a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto index 5bdd4f2bbb..674189b6ee 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto @@ -18,11 +18,12 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/3d/submaps_options_3d.proto"; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; -// NEXT ID: 18 +// NEXT ID: 19 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -52,15 +53,19 @@ message LocalTrajectoryBuilderOptions3D { mapping.proto.MotionFilterOptions motion_filter_options = 7; // Time constant in seconds for the orientation moving average based on - // observed gravity via the IMU. It should be chosen so that the error - // 1. from acceleration measurements not due to gravity (which gets worse when - // the constant is reduced) and - // 2. from integration of angular velocities (which gets worse when the - // constant is increased) is balanced. + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 15; // Number of histogram buckets for the rotational scan matcher. int32 rotational_histogram_size = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + SubmapsOptions3D submaps_options = 8; } diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 483f47f0d9..386e874351 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -59,7 +59,14 @@ TRAJECTORY_BUILDER_2D = { max_angle_radians = math.rad(1.), }, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, submaps = { num_range_data = 90, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 49567f4cf3..11955767b2 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -59,9 +59,17 @@ TRAJECTORY_BUILDER_3D = { max_angle_radians = 0.004, }, - imu_gravity_time_constant = 10., rotational_histogram_size = 120, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. + imu_gravity_time_constant = 10., + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, + submaps = { high_resolution = 0.10, high_resolution_max_range = 20., From d3794a420ab6b1f7e78f3058aba4f9914cab5fda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Thu, 30 Jul 2020 09:14:02 +0200 Subject: [PATCH 45/99] Extrapolator Result for motion compensation (#1731) This makes use of the ExtrapolationResult returned by the pose extrapolator. ExtrapolationResult contains poses corresponding to the timestamps of range data measurements to be used for motion compensation. When using the PoseExtrapolator implementation there is no functional difference. This PR prepares the integration of ImuBasedPoseExtrapolator, which requires using ExtrapolationResult returned by ExtrapolatePosesWithGravity to run efficiently. Signed-off-by: mschworer --- .../3d/local_trajectory_builder_3d.cc | 182 ++++++++++-------- .../internal/3d/local_trajectory_builder_3d.h | 8 +- 2 files changed, 107 insertions(+), 83 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 1f670e2582..3f34789871 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -25,6 +25,7 @@ #include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h" #include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h" #include "cartographer/mapping/proto/scan_matching//real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/transform/timestamped_transform.h" #include "glog/logging.h" namespace cartographer { @@ -55,7 +56,6 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D( options_.real_time_correlative_scan_matcher_options())), ceres_scan_matcher_(absl::make_unique( options_.ceres_scan_matcher_options())), - accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}}, range_data_collator_(expected_range_sensor_ids) {} LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {} @@ -116,14 +116,13 @@ std::unique_ptr LocalTrajectoryBuilder3D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { - const auto synchronized_data = + auto synchronized_data = range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); if (synchronized_data.ranges.empty()) { LOG(INFO) << "Range data collator filling buffer."; return nullptr; } - const common::Time& current_sensor_time = synchronized_data.time; if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -134,111 +133,134 @@ LocalTrajectoryBuilder3D::AddRangeData( CHECK(!synchronized_data.ranges.empty()); CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); const common::Time time_first_point = - current_sensor_time + + synchronized_data.time + common::FromSeconds(synchronized_data.ranges.front().point_time.time); if (time_first_point < extrapolator_->GetLastPoseTime()) { LOG(INFO) << "Extrapolator is still initializing."; return nullptr; } - std::vector hits = + if (num_accumulated_ == 0) { + accumulated_point_cloud_origin_data_.clear(); + } + + synchronized_data.ranges = sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) .Filter(synchronized_data.ranges); + accumulated_point_cloud_origin_data_.emplace_back( + std::move(synchronized_data)); + ++num_accumulated_; - std::vector hits_poses; - hits_poses.reserve(hits.size()); - bool warned = false; + if (num_accumulated_ < options_.num_accumulated_range_data()) { + return nullptr; + } + num_accumulated_ = 0; - for (const auto& hit : hits) { - common::Time time_point = - current_sensor_time + common::FromSeconds(hit.point_time.time); - if (time_point < extrapolator_->GetLastExtrapolatedTime()) { - if (!warned) { - LOG(ERROR) - << "Timestamp of individual range data point jumps backwards from " - << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; - warned = true; + bool warned = false; + std::vector hit_times; + common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + common::Time time_point = point_cloud_origin_data.time + + common::FromSeconds(hit.point_time.time); + if (time_point < prev_time_point) { + if (!warned) { + LOG(ERROR) << "Timestamp of individual range data point jumps " + "backwards from " + << prev_time_point << " to " << time_point; + warned = true; + } + time_point = prev_time_point; } - time_point = extrapolator_->GetLastExtrapolatedTime(); - } - hits_poses.push_back( - extrapolator_->ExtrapolatePose(time_point).cast()); - } - if (num_accumulated_ == 0) { - // 'accumulated_range_data_.origin' is not used. - accumulated_range_data_ = sensor::RangeData{{}, {}, {}}; + hit_times.push_back(time_point); + prev_time_point = time_point; + } } - - for (size_t i = 0; i < hits.size(); ++i) { - sensor::RangefinderPoint hit_in_local = - hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time); - const Eigen::Vector3f origin_in_local = - hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index); - const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; - const float range = delta.norm(); - if (range >= options_.min_range()) { - if (range <= options_.max_range()) { - accumulated_range_data_.returns.push_back(hit_in_local); - } else { - // We insert a ray cropped to 'max_range' as a miss for hits beyond the - // maximum range. This way the free space up to the maximum range will - // be updated. - hit_in_local.position = - origin_in_local + options_.max_range() / range * delta; - accumulated_range_data_.misses.push_back(hit_in_local); + hit_times.push_back(accumulated_point_cloud_origin_data_.back().time); + + const PoseExtrapolatorInterface::ExtrapolationResult extrapolation_result = + extrapolator_->ExtrapolatePosesWithGravity(hit_times); + std::vector hits_poses( + std::move(extrapolation_result.previous_poses)); + hits_poses.push_back(extrapolation_result.current_pose.cast()); + CHECK_EQ(hits_poses.size(), hit_times.size()); + + sensor::RangeData accumulated_range_data; + std::vector::const_iterator hits_poses_it = + hits_poses.begin(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + const Eigen::Vector3f hit_in_local = + *hits_poses_it * hit.point_time.position; + const Eigen::Vector3f origin_in_local = + *hits_poses_it * point_cloud_origin_data.origins.at(hit.origin_index); + const Eigen::Vector3f delta = hit_in_local - origin_in_local; + const float range = delta.norm(); + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + accumulated_range_data.returns.push_back( + sensor::RangefinderPoint{hit_in_local}); + } else { + // We insert a ray cropped to 'max_range' as a miss for hits beyond + // the maximum range. This way the free space up to the maximum range + // will be updated. + accumulated_range_data.misses.push_back(sensor::RangefinderPoint{ + origin_in_local + options_.max_range() / range * delta}); + } } + ++hits_poses_it; } } - ++num_accumulated_; - - if (num_accumulated_ >= options_.num_accumulated_range_data()) { - absl::optional sensor_duration; - if (last_sensor_time_.has_value()) { - sensor_duration = current_sensor_time - last_sensor_time_.value(); - } - last_sensor_time_ = current_sensor_time; - num_accumulated_ = 0; - - transform::Rigid3f current_pose = - extrapolator_->ExtrapolatePose(current_sensor_time).cast(); - - const auto voxel_filter_start = std::chrono::steady_clock::now(); - const sensor::RangeData filtered_range_data = { - current_pose.translation(), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data_.returns), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data_.misses)}; - const auto voxel_filter_stop = std::chrono::steady_clock::now(); - const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; + CHECK(std::next(hits_poses_it) == hits_poses.end()); - if (sensor_duration.has_value()) { - const double voxel_filter_fraction = - common::ToSeconds(voxel_filter_duration) / - common::ToSeconds(sensor_duration.value()); - kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); - } + const common::Time current_sensor_time = synchronized_data.time; + absl::optional sensor_duration; + if (last_sensor_time_.has_value()) { + sensor_duration = current_sensor_time - last_sensor_time_.value(); + } + last_sensor_time_ = current_sensor_time; + + const common::Time current_time = hit_times.back(); + const auto voxel_filter_start = std::chrono::steady_clock::now(); + const sensor::RangeData filtered_range_data = { + extrapolation_result.current_pose.translation().cast(), + sensor::VoxelFilter(options_.voxel_filter_size()) + .Filter(accumulated_range_data.returns), + sensor::VoxelFilter(options_.voxel_filter_size()) + .Filter(accumulated_range_data.misses)}; + const auto voxel_filter_stop = std::chrono::steady_clock::now(); + const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; - return AddAccumulatedRangeData( - current_sensor_time, - sensor::TransformRangeData(filtered_range_data, current_pose.inverse()), - sensor_duration); + if (sensor_duration.has_value()) { + const double voxel_filter_fraction = + common::ToSeconds(voxel_filter_duration) / + common::ToSeconds(sensor_duration.value()); + kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); } - return nullptr; + + return AddAccumulatedRangeData( + current_time, + sensor::TransformRangeData( + filtered_range_data, + extrapolation_result.current_pose.inverse().cast()), + sensor_duration, extrapolation_result.current_pose, + extrapolation_result.gravity_from_tracking); } std::unique_ptr LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& filtered_range_data_in_tracking, - const absl::optional& sensor_duration) { + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment) { if (filtered_range_data_in_tracking.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; } - const transform::Rigid3d pose_prediction = - extrapolator_->ExtrapolatePose(time); const auto scan_matcher_start = std::chrono::steady_clock::now(); @@ -278,8 +300,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction); } - const Eigen::Quaterniond gravity_alignment = - extrapolator_->EstimateGravityOrientation(time); sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData( filtered_range_data_in_tracking, pose_estimate->cast()); diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index b0ee05bc7d..69f034eed9 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -33,6 +33,7 @@ #include "cartographer/sensor/internal/voxel_filter.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -78,7 +79,9 @@ class LocalTrajectoryBuilder3D { std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& filtered_range_data_in_tracking, - const absl::optional& sensor_duration); + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment); std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& filtered_range_data_in_local, @@ -106,7 +109,8 @@ class LocalTrajectoryBuilder3D { std::unique_ptr extrapolator_; int num_accumulated_ = 0; - sensor::RangeData accumulated_range_data_; + std::vector + accumulated_point_cloud_origin_data_; absl::optional last_wall_time_; absl::optional last_thread_cpu_time_seconds_; From 6c889490e245cc5d9da15023249c6fc7119def3f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Thu, 30 Jul 2020 13:52:08 +0200 Subject: [PATCH 46/99] Integrate imu based extrapolator (#1732) This integrates the imu based extrapolator implementation, adds lua configuration and conversions from lua to proto. Note that the parameters of the imu based extrapolator are not tuned yet and the default is still the constant-velocity extrapolator. Signed-off-by: mschworer --- .../3d/local_trajectory_builder_3d.cc | 12 ++++- .../3d/local_trajectory_builder_3d_test.cc | 16 +++++++ .../mapping/pose_extrapolator_interface.cc | 47 +++++++++++++++++-- .../mapping/pose_extrapolator_interface.h | 6 ++- .../local_trajectory_builder_options_2d.proto | 12 ++--- .../local_trajectory_builder_options_3d.proto | 19 +++++--- configuration_files/trajectory_builder_2d.lua | 16 +++++++ configuration_files/trajectory_builder_3d.lua | 17 +++++++ 8 files changed, 125 insertions(+), 20 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 3f34789871..07c5aec2b4 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -108,8 +108,18 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { extrapolator_->AddImuData(imu_data); return; } + std::vector initial_poses; + for (const auto& pose_proto : options_.initial_poses()) { + initial_poses.push_back(transform::FromProto(pose_proto)); + } + std::vector initial_imu_data; + for (const auto& imu : options_.initial_imu_data()) { + initial_imu_data.push_back(sensor::FromProto(imu)); + } + initial_imu_data.push_back(imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( - options_.pose_extrapolator_options(), {imu_data}); + options_.pose_extrapolator_options(), initial_imu_data, initial_poses); } std::unique_ptr diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 0558b0d053..22ec9c9e8d 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -98,10 +98,26 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { rotational_histogram_size = 120, pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = { diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index 32a5abd884..413a8da442 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -16,7 +16,9 @@ #include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/imu_based_pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h" namespace cartographer { @@ -35,27 +37,64 @@ CreateConstantVelocityPoseExtrapolatorOptions( return options; } +proto::ImuBasedPoseExtrapolatorOptions CreateImuBasedPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ImuBasedPoseExtrapolatorOptions options; + + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_gravity_constant( + parameter_dictionary->GetDouble("gravity_constant")); + options.set_pose_translation_weight( + parameter_dictionary->GetDouble("pose_translation_weight")); + options.set_pose_rotation_weight( + parameter_dictionary->GetDouble("pose_rotation_weight")); + options.set_imu_acceleration_weight( + parameter_dictionary->GetDouble("imu_acceleration_weight")); + options.set_imu_rotation_weight( + parameter_dictionary->GetDouble("imu_rotation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + *options.mutable_solver_options() = CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("solver_options").get()); + + return options; +} + } // namespace proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::PoseExtrapolatorOptions options; + options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based")); *options.mutable_constant_velocity() = CreateConstantVelocityPoseExtrapolatorOptions( parameter_dictionary->GetDictionary("constant_velocity").get()); + *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("imu_based").get()); + return options; } std::unique_ptr PoseExtrapolatorInterface::CreateWithImuData( const proto::PoseExtrapolatorOptions& options, - const std::vector& imu_data) { + const std::vector& imu_data, + const std::vector& initial_poses) { CHECK(!imu_data.empty()); // TODO(schwoere): Implement/integrate imu based extrapolator. CHECK(!options.use_imu_based()) << "Not implemented!"; - return PoseExtrapolator::InitializeWithImu( - common::FromSeconds(options.constant_velocity().pose_queue_duration()), - options.constant_velocity().imu_gravity_time_constant(), imu_data.back()); + if (options.use_imu_based()) { + return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(), + imu_data, initial_poses); + } else { + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); + } } } // namespace mapping diff --git a/cartographer/mapping/pose_extrapolator_interface.h b/cartographer/mapping/pose_extrapolator_interface.h index 6f470db6fc..bf258ce677 100644 --- a/cartographer/mapping/pose_extrapolator_interface.h +++ b/cartographer/mapping/pose_extrapolator_interface.h @@ -45,13 +45,15 @@ class PoseExtrapolatorInterface { }; PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; - PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete; + PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = + delete; virtual ~PoseExtrapolatorInterface() {} // TODO: Remove dependency cycle. static std::unique_ptr CreateWithImuData( const proto::PoseExtrapolatorOptions& options, - const std::vector& imu_data); + const std::vector& imu_data, + const std::vector& initial_poses); // Returns the time of the last added pose or Time::min() if no pose was added // yet. diff --git a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto index 0005c513f1..470dfe81c6 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto @@ -60,12 +60,12 @@ message LocalTrajectoryBuilderOptions2D { MotionFilterOptions motion_filter_options = 13; // Time constant in seconds for the orientation moving average based on - // observed gravity via the IMU. It should be chosen so that the error - // 1. from acceleration measurements not due to gravity (which gets worse when - // the constant is reduced) and - // 2. from integration of angular velocities (which gets worse when the - // constant is increased) is balanced. - // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 17; mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; diff --git a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto index 674189b6ee..7039ddf817 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto @@ -22,8 +22,10 @@ import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/timestamped_transform.proto"; -// NEXT ID: 19 +// NEXT ID: 21 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -53,12 +55,12 @@ message LocalTrajectoryBuilderOptions3D { mapping.proto.MotionFilterOptions motion_filter_options = 7; // Time constant in seconds for the orientation moving average based on - // observed gravity via the IMU. It should be chosen so that the error - // 1. from acceleration measurements not due to gravity (which gets worse when - // the constant is reduced) and - // 2. from integration of angular velocities (which gets worse when the - // constant is increased) is balanced. - // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 15; @@ -67,5 +69,8 @@ message LocalTrajectoryBuilderOptions3D { mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + repeated transform.proto.TimestampedTransform initial_poses = 19; + repeated sensor.proto.ImuData initial_imu_data = 20; + SubmapsOptions3D submaps_options = 8; } diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 386e874351..373b099908 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -62,10 +62,26 @@ TRAJECTORY_BUILDER_2D = { -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = { diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 11955767b2..3bd3dd8eca 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -64,10 +64,27 @@ TRAJECTORY_BUILDER_3D = { -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + -- TODO(wohe): Tune these parameters on the example datasets. + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = { From 19a6eab07a9bf941da836e90c20905f0fad7446c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 14 Aug 2020 11:08:53 +0200 Subject: [PATCH 47/99] Change pbstream migrate to support new submap format. (#1733) This changes the migration support to: 1. Remove migration from old files from before version 1.0. If this is still needed by someone, they can use the tool as it was before this change. 2. Adds migration of maps without submap histograms. These were so far migrated on-the-fly, but #1710 aims to remove this. See also #1709. This was tested by converting a serialized 3D map and verifying it can still be used for localization. The file changed a little bit, but it seems to be good enough. Signed-off-by: Wolfgang Hess --- cartographer/io/internal/pbstream_migrate.cc | 21 +- cartographer/io/pbstream_main.cc | 6 +- .../io/serialization_format_migration.cc | 410 +++++++----------- .../io/serialization_format_migration.h | 11 +- .../io/serialization_format_migration_test.cc | 144 ------ docs/source/pbstream_migration.rst | 28 +- 6 files changed, 198 insertions(+), 422 deletions(-) diff --git a/cartographer/io/internal/pbstream_migrate.cc b/cartographer/io/internal/pbstream_migrate.cc index a84af06656..43d6b8a4c9 100644 --- a/cartographer/io/internal/pbstream_migrate.cc +++ b/cartographer/io/internal/pbstream_migrate.cc @@ -21,19 +21,18 @@ #include "gflags/gflags.h" #include "glog/logging.h" -DEFINE_bool(migrate_grid_format, false, - "Set if the submap data of the input pbstream uses the old " - "probability grid format."); +DEFINE_bool(include_unfinished_submaps, true, + "Whether to include unfinished submaps in the output."); namespace cartographer { namespace io { int pbstream_migrate(int argc, char** argv) { std::stringstream ss; - ss << "\n\nTool for migrating files that use the serialization output of " - "Cartographer 0.3, to the new serialization format, which includes a " - "header (Version 1). You may need to specify the '--migrate_grid_format" - " flag if the input file contains submaps with the legacy grid format." + ss << "\n\nTool for migrating files that use submaps without histograms " + "to the new submap format, which includes a histogram. You can " + "set --include_unfinished_submaps to false if you want to exclude " + "unfinished submaps in the output." << "\nUsage: " << argv[0] << " " << argv[1] << " [flags]"; google::SetUsageMessage(ss.str()); @@ -44,10 +43,10 @@ int pbstream_migrate(int argc, char** argv) { } cartographer::io::ProtoStreamReader input(argv[2]); cartographer::io::ProtoStreamWriter output(argv[3]); - LOG(INFO) << "Migrating old serialization format in \"" << argv[2] - << "\" to new serialization format in \"" << argv[3] << "\""; - cartographer::io::MigrateStreamFormatToVersion1(&input, &output, - FLAGS_migrate_grid_format); + LOG(INFO) << "Migrating serialization format 1 in \"" << argv[2] + << "\" to serialization format 2 in \"" << argv[3] << "\""; + cartographer::io::MigrateStreamVersion1ToVersion2( + &input, &output, FLAGS_include_unfinished_submaps); CHECK(output.Close()) << "Could not write migrated pbstream file to: " << argv[3]; diff --git a/cartographer/io/pbstream_main.cc b/cartographer/io/pbstream_main.cc index 0a795ff9d7..4fffbd4052 100644 --- a/cartographer/io/pbstream_main.cc +++ b/cartographer/io/pbstream_main.cc @@ -30,12 +30,12 @@ int main(int argc, char** argv) { "Swiss Army knife for pbstreams.\n\n" "Currently supported subcommands are:\n" "\tinfo - Prints summary of pbstream.\n" - "\tmigrate - Migrates old pbstream (w/o header) to new pbstream format."; + "\tmigrate - Migrates pbstream to the new submap format."; google::ParseCommandLineFlags(&argc, &argv, true); if (argc < 2) { google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } else if (std::string(argv[1]) == "info") { return ::cartographer::io::pbstream_info(argc, argv); @@ -44,7 +44,7 @@ int main(int argc, char** argv) { } else { LOG(INFO) << "Unknown subtool: \"" << argv[1]; google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } } diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc index 186fb92aea..fb3c222ce7 100644 --- a/cartographer/io/serialization_format_migration.cc +++ b/cartographer/io/serialization_format_migration.cc @@ -18,284 +18,198 @@ #include -#include "absl/container/flat_hash_map.h" +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" -#include "cartographer/mapping/proto/internal/legacy_submap.pb.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "glog/logging.h" namespace cartographer { namespace io { -namespace { using mapping::proto::SerializedData; -using ProtoMap = absl::flat_hash_map>; -bool ReadPoseGraph(cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - auto& pose_graph_vec = (*proto_map)[SerializedData::kPoseGraph]; - pose_graph_vec.emplace_back(); - return input->ReadProto(pose_graph_vec.back().mutable_pose_graph()); -} - -bool ReadBuilderOptions( +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - auto& options_vec = - (*proto_map)[SerializedData::kAllTrajectoryBuilderOptions]; - options_vec.emplace_back(); - return input->ReadProto( - options_vec.back().mutable_all_trajectory_builder_options()); -} - -mapping::proto::Submap MigrateLegacySubmap2d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap2D submap_2d; - - // Convert probability grid to generalized grid. - *submap_2d.mutable_grid()->mutable_limits() = - submap_in.submap_2d().probability_grid().limits(); - *submap_2d.mutable_grid()->mutable_cells() = - submap_in.submap_2d().probability_grid().cells(); - for (auto& cell : *submap_2d.mutable_grid()->mutable_cells()) { - cell = -1 * cell; - } - // CellBox can't be trivially copied because the protobuf - // serialization field number doesn't match. - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_x( - submap_in.submap_2d().probability_grid().known_cells_box().max_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_y( - submap_in.submap_2d().probability_grid().known_cells_box().max_y()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_x( - submap_in.submap_2d().probability_grid().known_cells_box().min_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_y( - submap_in.submap_2d().probability_grid().known_cells_box().min_y()); + cartographer::io::ProtoStreamWriterInterface* const output, + bool include_unfinished_submaps) { + auto file_resolver = ::absl::make_unique( + std::vector{std::string(common::kSourceDirectory) + + "/configuration_files"}); + const std::string kCode = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_3d = true + return MAP_BUILDER)text"; + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + const auto options = + mapping::CreateMapBuilderOptions(&lua_parameter_dictionary); - // Correspondence costs can be safely set to standard values. - // Otherwise, this would be done during the next deserialization, but with a - // warning, which we can avoid by setting it already here. - submap_2d.mutable_grid()->set_max_correspondence_cost( - mapping::kMaxCorrespondenceCost); - submap_2d.mutable_grid()->set_min_correspondence_cost( - mapping::kMinCorrespondenceCost); - submap_2d.mutable_grid()->mutable_probability_grid_2d(); - *submap_2d.mutable_local_pose() = submap_in.submap_2d().local_pose(); - submap_2d.set_num_range_data(submap_in.submap_2d().num_range_data()); - submap_2d.set_finished(submap_in.submap_2d().finished()); + common::ThreadPool thread_pool(1); + CHECK(!options.use_trajectory_builder_2d()); + // We always use 3D here. 2D submaps do not have histograms. + mapping::PoseGraph3D pose_graph( + options.pose_graph_options(), + absl::make_unique( + options.pose_graph_options().optimization_problem_options()), + &thread_pool); - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_2d() = submap_2d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} + ProtoStreamDeserializer deserializer(input); -mapping::proto::Submap MigrateLegacySubmap3d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap3D submap_3d; - *submap_3d.mutable_local_pose() = submap_in.submap_3d().local_pose(); - submap_3d.set_num_range_data(submap_in.submap_3d().num_range_data()); - submap_3d.set_finished(submap_in.submap_3d().finished()); - *submap_3d.mutable_high_resolution_hybrid_grid() = - submap_in.submap_3d().high_resolution_hybrid_grid(); - *submap_3d.mutable_low_resolution_hybrid_grid() = - submap_in.submap_3d().low_resolution_hybrid_grid(); + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_3d() = submap_3d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} + std::vector + trajectory_builder_options; + for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { + auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids(i); + trajectory_builder_options.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_proto.trajectory_id(), i); + } -bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - mapping::proto::LegacySerializedData legacy_data; - if (!input->ReadProto(&legacy_data)) return false; + // Apply the calculated remapping to constraints in the pose graph proto. + for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { + constraint_proto.mutable_submap_id()->set_trajectory_id( + constraint_proto.submap_id().trajectory_id()); + constraint_proto.mutable_node_id()->set_trajectory_id( + constraint_proto.node_id().trajectory_id()); + } - if (legacy_data.has_submap()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().grid().has_probability_grid_2d() || - legacy_data.submap().submap_2d().grid().has_tsdf_2d()) - << "\nThe legacy data contains a 2D submap, but it's not using the " - "expected grid format. Try to migrate the grid format instead."; + mapping::MapById submap_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Submap& submap_proto : + trajectory_proto.submap()) { + submap_poses.Insert(mapping::SubmapId{trajectory_proto.trajectory_id(), + submap_proto.submap_index()}, + transform::ToRigid3(submap_proto.pose())); } - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_submap() = legacy_data.submap(); - } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_node() = legacy_data.node(); } - if (legacy_data.has_trajectory_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating trajectory data."; - auto& output_vector = - (*proto_map)[SerializedData::kTrajectoryDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_trajectory_data() = - legacy_data.trajectory_data(); - } - if (legacy_data.has_imu_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating IMU data."; - auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_imu_data() = legacy_data.imu_data(); - } - if (legacy_data.has_odometry_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating odometry data."; - auto& output_vector = (*proto_map)[SerializedData::kOdometryData]; - output_vector.emplace_back(); - *output_vector.back().mutable_odometry_data() = legacy_data.odometry_data(); - } - if (legacy_data.has_fixed_frame_pose_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating fixed frame pose data."; - auto& output_vector = - (*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_fixed_frame_pose_data() = - legacy_data.fixed_frame_pose_data(); - } - if (legacy_data.has_landmark_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating landmark data."; - auto& output_vector = - (*proto_map)[SerializedData::kLandmarkDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_landmark_data() = legacy_data.landmark_data(); - } - return true; -} -bool DeserializeNextAndMigrateGridFormat( - cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - mapping::proto::LegacySerializedDataLegacySubmap legacy_data; - if (!input->ReadProto(&legacy_data)) return false; - - if (legacy_data.has_submap()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().has_probability_grid()) - << "\nThe legacy data contains a 2D submap, but it has no legacy " - "probability grid that can be migrated to the new grid format."; - *output_vector.back().mutable_submap() = - MigrateLegacySubmap2d(legacy_data.submap()); - } else { - *output_vector.back().mutable_submap() = - MigrateLegacySubmap3d(legacy_data.submap()); + mapping::MapById node_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Node& node_proto : + trajectory_proto.node()) { + node_poses.Insert(mapping::NodeId{trajectory_proto.trajectory_id(), + node_proto.node_index()}, + transform::ToRigid3(node_proto.pose())); } } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_node() = legacy_data.node(); - } - if (legacy_data.has_trajectory_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating trajectory data."; - auto& output_vector = - (*proto_map)[SerializedData::kTrajectoryDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_trajectory_data() = - legacy_data.trajectory_data(); - } - if (legacy_data.has_imu_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating IMU data."; - auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_imu_data() = legacy_data.imu_data(); - } - if (legacy_data.has_odometry_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating odometry data."; - auto& output_vector = (*proto_map)[SerializedData::kOdometryData]; - output_vector.emplace_back(); - *output_vector.back().mutable_odometry_data() = legacy_data.odometry_data(); - } - if (legacy_data.has_fixed_frame_pose_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating fixed frame pose data."; - auto& output_vector = - (*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_fixed_frame_pose_data() = - legacy_data.fixed_frame_pose_data(); - } - if (legacy_data.has_landmark_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating landmark data."; - auto& output_vector = - (*proto_map)[SerializedData::kLandmarkDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_landmark_data() = legacy_data.landmark_data(); - } - return true; -} -ProtoMap ParseLegacyData( - cartographer::io::ProtoStreamReaderInterface* const input, - bool migrate_grid_format) { - ProtoMap proto_map; - CHECK(ReadPoseGraph(input, &proto_map)) - << "Input stream seems to differ from original stream format. Could " - "not " - "read PoseGraph as first message."; - CHECK(ReadBuilderOptions(input, &proto_map)) - << "Input stream seems to differ from original stream format. Could " - "not " - "read AllTrajectoryBuilderOptions as second message."; - if (migrate_grid_format) { - do { - } while (DeserializeNextAndMigrateGridFormat(input, &proto_map)); - } else { - do { - } while (DeserializeNext(input, &proto_map)); + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmark_poses()) { + pose_graph.SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose()), + true); } - return proto_map; -} -mapping::proto::SerializationHeader CreateSerializationHeader() { - constexpr uint32_t kVersion1 = 1; - mapping::proto::SerializationHeader header; - header.set_format_version(kVersion1); - return header; -} + mapping::MapById + submap_id_to_submap; + mapping::MapById node_id_to_node; + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(FATAL) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(FATAL) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + case SerializedData::kSubmap: { + CHECK(proto.submap().has_submap_3d()) + << "Converting to the new submap format only makes sense for 3D."; + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + proto.submap().submap_id().trajectory_id()); + submap_id_to_submap.Insert( + mapping::SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}, + proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + proto.node().node_id().trajectory_id()); + const mapping::NodeId node_id(proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()); + const transform::Rigid3d& node_pose = node_poses.at(node_id); + pose_graph.AddNodeFromProto(node_pose, proto.node()); + node_id_to_node.Insert(node_id, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + proto.trajectory_data().trajectory_id()); + pose_graph.SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + pose_graph.AddImuData(proto.imu_data().trajectory_id(), + sensor::FromProto(proto.imu_data().imu_data())); + break; + } + case SerializedData::kOdometryData: { + pose_graph.AddOdometryData( + proto.odometry_data().trajectory_id(), + sensor::FromProto(proto.odometry_data().odometry_data())); + break; + } + case SerializedData::kFixedFramePoseData: { + pose_graph.AddFixedFramePoseData( + proto.fixed_frame_pose_data().trajectory_id(), + sensor::FromProto( + proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; + } + case SerializedData::kLandmarkData: { + pose_graph.AddLandmarkData( + proto.landmark_data().trajectory_id(), + sensor::FromProto(proto.landmark_data().landmark_data())); + break; + } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + } -void SerializeToVersion1Format( - const ProtoMap& deserialized_data, - cartographer::io::ProtoStreamWriterInterface* const output) { - const std::vector kFieldSerializationOrder = { - SerializedData::kPoseGraphFieldNumber, - SerializedData::kAllTrajectoryBuilderOptionsFieldNumber, - SerializedData::kSubmapFieldNumber, - SerializedData::kNodeFieldNumber, - SerializedData::kTrajectoryDataFieldNumber, - SerializedData::kImuDataFieldNumber, - SerializedData::kOdometryDataFieldNumber, - SerializedData::kFixedFramePoseDataFieldNumber, - SerializedData::kLandmarkDataFieldNumber}; + // TODO(schwoere): Remove backwards compatibility once the pbstream format + // version 2 is established. + if (deserializer.header().format_version() == + kFormatVersionWithoutSubmapHistograms) { + submap_id_to_submap = MigrateSubmapFormatVersion1ToVersion2( + submap_id_to_submap, node_id_to_node, pose_graph_proto); + } - LOG(INFO) << "Writing proto stream."; - output->WriteProto(CreateSerializationHeader()); - for (auto field_index : kFieldSerializationOrder) { - const auto proto_vector_it = deserialized_data.find(field_index); - if (proto_vector_it == deserialized_data.end()) continue; - for (const auto& proto : proto_vector_it->second) { - output->WriteProto(proto); - } + for (const auto& submap_id_submap : submap_id_to_submap) { + pose_graph.AddSubmapFromProto(submap_poses.at(submap_id_submap.id), + submap_id_submap.data); } -} -} // namespace -void MigrateStreamFormatToVersion1( - cartographer::io::ProtoStreamReaderInterface* const input, - cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format) { - SerializeToVersion1Format(ParseLegacyData(input, migrate_grid_format), - output); + pose_graph.AddSerializedConstraints( + mapping::FromProto(pose_graph_proto.constraint())); + CHECK(input->eof()); + + WritePbStream(pose_graph, trajectory_builder_options, output, + include_unfinished_submaps); } mapping::MapById diff --git a/cartographer/io/serialization_format_migration.h b/cartographer/io/serialization_format_migration.h index 3570a8ac0f..32e101dd51 100644 --- a/cartographer/io/serialization_format_migration.h +++ b/cartographer/io/serialization_format_migration.h @@ -24,14 +24,13 @@ namespace cartographer { namespace io { -// This helper function, migrates the input stream, which is supposed to match -// to the "old" stream format order (PoseGraph, AllTrajectoryBuilderOptions, -// SerializedData*) to the version 1 stream format (SerializationHeader, -// SerializedData*). -void MigrateStreamFormatToVersion1( +// This helper function migrates the input stream, which is supposed +// to contain submaps without histograms (stream format version 1) to +// an output stream containing submaps with histograms (version 2). +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format); + bool include_unfinished_submaps); mapping::MapById MigrateSubmapFormatVersion1ToVersion2( diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc index 16e29b2bc3..19e3535754 100644 --- a/cartographer/io/serialization_format_migration_test.cc +++ b/cartographer/io/serialization_format_migration_test.cc @@ -21,7 +21,6 @@ #include "cartographer/io/internal/in_memory_proto_stream.h" #include "cartographer/mapping/internal/testing/test_helpers.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" @@ -35,74 +34,6 @@ namespace cartographer { namespace io { namespace { -using ::google::protobuf::TextFormat; -using ::testing::Eq; -using ::testing::SizeIs; - -class MigrationTest : public ::testing::Test { - private: - template - void AddLegacyDataToReader(InMemoryProtoStreamReader& reader) { - mapping::proto::PoseGraph pose_graph; - mapping::proto::AllTrajectoryBuilderOptions all_options; - LegacySerializedDataType submap; - submap.mutable_submap(); - LegacySerializedDataType node; - node.mutable_node(); - LegacySerializedDataType imu_data; - imu_data.mutable_imu_data(); - LegacySerializedDataType odometry_data; - odometry_data.mutable_odometry_data(); - LegacySerializedDataType fixed_frame_pose; - fixed_frame_pose.mutable_fixed_frame_pose_data(); - LegacySerializedDataType trajectory_data; - trajectory_data.mutable_trajectory_data(); - LegacySerializedDataType landmark_data; - landmark_data.mutable_landmark_data(); - - reader.AddProto(pose_graph); - reader.AddProto(all_options); - reader.AddProto(submap); - reader.AddProto(node); - reader.AddProto(imu_data); - reader.AddProto(odometry_data); - reader.AddProto(fixed_frame_pose); - reader.AddProto(trajectory_data); - reader.AddProto(landmark_data); - } - - protected: - void SetUp() override { - AddLegacyDataToReader(reader_); - AddLegacyDataToReader( - reader_for_migrating_grid_); - - writer_.reset(new ForwardingProtoStreamWriter( - [this](const google::protobuf::Message* proto) -> bool { - std::string msg_string; - TextFormat::PrintToString(*proto, &msg_string); - this->output_messages_.push_back(msg_string); - return true; - })); - writer_for_migrating_grid_.reset(new ForwardingProtoStreamWriter( - [this](const google::protobuf::Message* proto) -> bool { - std::string msg_string; - TextFormat::PrintToString(*proto, &msg_string); - this->output_messages_after_migrating_grid_.push_back(msg_string); - return true; - })); - } - - InMemoryProtoStreamReader reader_; - InMemoryProtoStreamReader reader_for_migrating_grid_; - std::unique_ptr writer_; - std::unique_ptr writer_for_migrating_grid_; - std::vector output_messages_; - std::vector output_messages_after_migrating_grid_; - - static constexpr int kNumOriginalMessages = 9; -}; - class SubmapHistogramMigrationTest : public ::testing::Test { protected: void SetUp() override { @@ -141,81 +72,6 @@ class SubmapHistogramMigrationTest : public ::testing::Test { mapping::proto::Node node_; }; -TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - // We expect one message more than the original number of messages, because of - // the added header. - EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); - - mapping::proto::SerializationHeader header; - EXPECT_TRUE(TextFormat::ParseFromString(output_messages_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1u)); -} - -TEST_F(MigrationTest, MigrationWithGridMigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - // We expect one message more than the original number of messages, because of - // the added header. - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - mapping::proto::SerializationHeader header; - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1u)); -} - -TEST_F(MigrationTest, SerializedDataOrderIsCorrect) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); - - std::vector serialized( - output_messages_.size() - 1); - for (size_t i = 1; i < output_messages_.size(); ++i) { - EXPECT_TRUE( - TextFormat::ParseFromString(output_messages_[i], &serialized[i - 1])); - } - - EXPECT_TRUE(serialized[0].has_pose_graph()); - EXPECT_TRUE(serialized[1].has_all_trajectory_builder_options()); - EXPECT_TRUE(serialized[2].has_submap()); - EXPECT_TRUE(serialized[3].has_node()); - EXPECT_TRUE(serialized[4].has_trajectory_data()); - EXPECT_TRUE(serialized[5].has_imu_data()); - EXPECT_TRUE(serialized[6].has_odometry_data()); - EXPECT_TRUE(serialized[7].has_fixed_frame_pose_data()); - EXPECT_TRUE(serialized[8].has_landmark_data()); -} - -TEST_F(MigrationTest, SerializedDataOrderAfterGridMigrationIsCorrect) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - std::vector serialized( - output_messages_after_migrating_grid_.size() - 1); - for (size_t i = 1; i < output_messages_after_migrating_grid_.size(); ++i) { - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[i], &serialized[i - 1])); - } - - EXPECT_TRUE(serialized[0].has_pose_graph()); - EXPECT_TRUE(serialized[1].has_all_trajectory_builder_options()); - EXPECT_TRUE(serialized[2].has_submap()); - EXPECT_TRUE(serialized[3].has_node()); - EXPECT_TRUE(serialized[4].has_trajectory_data()); - EXPECT_TRUE(serialized[5].has_imu_data()); - EXPECT_TRUE(serialized[6].has_odometry_data()); - EXPECT_TRUE(serialized[7].has_fixed_frame_pose_data()); - EXPECT_TRUE(serialized[8].has_landmark_data()); -} - TEST_F(SubmapHistogramMigrationTest, SubmapHistogramGenerationFromTrajectoryNodes) { mapping::MapById diff --git a/docs/source/pbstream_migration.rst b/docs/source/pbstream_migration.rst index f2699f31b1..20ce54350e 100644 --- a/docs/source/pbstream_migration.rst +++ b/docs/source/pbstream_migration.rst @@ -16,22 +16,30 @@ Migration tool for pbstream files ================================= -With the update of the pbstream serialization format as discussed in -`RFC-0021`_, previously serialized pbstream files are not loadable in -Cartographer 1.0 anymore. - -In order to enable users to reuse previously generated pbstream files, we -provide a migration tool which converts pbstreams from Cartographer 0.3 to the -new serialization format used in Cartographer 1.0. +The pbstream serialization format for 3D has changed to include additional +data (histograms) in each submap. Code to load old data by migrating +on-the-fly will be removed soon. Once this happened, users who wish to +migrate old pbstream files can use a migration tool. The tool is shipped as part of Cartographer's pbstream tool (`source`_) and once built can be invoked as follows::: cartographer_pbstream migrate old.pbstream new.pbstream -The tool assumes that the first pbstream provided as commandline argument, -follows the serialization format of Cartographer 0.3. The resulting -1.0 pbstream will be saved to the second commandline argument location. +The tool assumes 3D data in the old submap format as input and converts it +to the currently used format version. + +Migrating pre-1.0 pbstream files +================================ + +With the update of the pbstream serialization format as discussed in +`RFC-0021`_, previously serialized pbstream files are not loadable in +Cartographer 1.0 anymore. + +In order to enable users to reuse previously generated pbstream files, +migration using an older version of the migration tool is necessary. +The current tool does not support this migration anymore. Please use +the version at Git SHA 6c889490e245cc5d9da15023249c6fc7119def3f. .. _RFC-0021: https://github.com/cartographer-project/rfcs/blob/master/text/0021-serialization-format.md .. _source: https://github.com/cartographer-project/cartographer/blob/master/cartographer/io/pbstream_main.cc From 9b405c3dc8a422ea2d7fd7a8349d2ff609ba5ff5 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 17 Aug 2020 19:34:35 +0200 Subject: [PATCH 48/99] Deactivate memory-critical histogram backwards compatibility. (#1710) It's essentially leading to buffering all submaps twice, which is a high (~2x) peak in memory consumption during deserialization. Don't convert anymore since this affects _all_ pbstreams and not only the (probably few) old pbstreams that require the conversion. Signed-off-by: Michael Grupp --- .../cloud/internal/client_server_test.cc | 2 +- cartographer/mapping/map_builder.cc | 33 +++++++------------ 2 files changed, 13 insertions(+), 22 deletions(-) diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 90c2dd7f46..8a089710b3 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -55,7 +55,7 @@ constexpr double kDuration = 4.; // Seconds. constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTravelDistance = 1.2; // Meters. -constexpr char kSerializationHeaderProtoString[] = "format_version: 1"; +constexpr char kSerializationHeaderProtoString[] = "format_version: 2"; constexpr char kPoseGraphProtoString[] = R"(pose_graph { trajectory: { trajectory_id: 0 diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 331ee6ee58..c93cd12cad 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -285,8 +285,14 @@ std::map MapBuilder::LoadState( true); } - MapById submap_id_to_submap; - MapById node_id_to_node; + if (options_.use_trajectory_builder_3d()) { + CHECK_NE(deserializer.header().format_version(), + io::kFormatVersionWithoutSubmapHistograms) + << "The pbstream file contains submaps without rotational histograms. " + "This can be converted with the 'pbstream migrate' tool, see the " + "Cartographer documentation for details. "; + } + SerializedData proto; while (deserializer.ReadNextSerializedData(&proto)) { switch (proto.data_case()) { @@ -303,10 +309,10 @@ std::map MapBuilder::LoadState( proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( trajectory_remapping.at( proto.submap().submap_id().trajectory_id())); - submap_id_to_submap.Insert( - SubmapId{proto.submap().submap_id().trajectory_id(), - proto.submap().submap_id().submap_index()}, - proto.submap()); + const SubmapId submap_id(proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()); + pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id), + proto.submap()); break; } case SerializedData::kNode: { @@ -316,7 +322,6 @@ std::map MapBuilder::LoadState( proto.node().node_id().node_index()); const transform::Rigid3d& node_pose = node_poses.at(node_id); pose_graph_->AddNodeFromProto(node_pose, proto.node()); - node_id_to_node.Insert(node_id, proto.node()); break; } case SerializedData::kTrajectoryData: { @@ -361,20 +366,6 @@ std::map MapBuilder::LoadState( } } - // TODO(schwoere): Remove backwards compatibility once the pbstream format - // version 2 is established. - if (deserializer.header().format_version() == - io::kFormatVersionWithoutSubmapHistograms) { - submap_id_to_submap = - cartographer::io::MigrateSubmapFormatVersion1ToVersion2( - submap_id_to_submap, node_id_to_node, pose_graph_proto); - } - - for (const auto& submap_id_submap : submap_id_to_submap) { - pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id), - submap_id_submap.data); - } - if (load_frozen_state) { // Add information about which nodes belong to which submap. // Required for 3D pure localization. From 1c8c1d144ecf0a78b1f26b13d37d110ff0d1a8e0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 20 Aug 2020 11:07:25 +0200 Subject: [PATCH 49/99] Remove struct TimedRangeData. (#1735) It was only used in a unit test which can be improved by removing it. Signed-off-by: Wolfgang Hess --- .../3d/local_trajectory_builder_3d_test.cc | 15 +++++++-------- cartographer/sensor/range_data.cc | 16 ---------------- cartographer/sensor/range_data.h | 14 -------------- 3 files changed, 7 insertions(+), 38 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 22ec9c9e8d..381fde2153 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -197,7 +197,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) { + sensor::TimedPointCloudData GeneratePointCloudData( + const transform::Rigid3d& pose, const common::Time time) { // 360 degree rays at 16 angles. sensor::TimedPointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { @@ -237,10 +238,9 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { 0.}; returns_in_world_frame.push_back(return_point); } - return {Eigen::Vector3f::Zero(), + return {time, Eigen::Vector3f::Zero(), sensor::TransformTimedPointCloud(returns_in_world_frame, - pose.inverse().cast()), - {}}; + pose.inverse().cast())}; } void AddLinearOnlyImuObservation(const common::Time time, @@ -278,11 +278,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - const auto range_data = GenerateRangeData(node.pose); + const auto point_cloud = GeneratePointCloudData(node.pose, node.time); const std::unique_ptr - matching_result = local_trajectory_builder_->AddRangeData( - kSensorId, sensor::TimedPointCloudData{ - node.time, range_data.origin, range_data.returns}); + matching_result = + local_trajectory_builder_->AddRangeData(kSensorId, point_cloud); if (matching_result != nullptr) { EXPECT_THAT(matching_result->local_pose, transform::IsNearly(node.pose, 1e-1)); diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 2eea185554..1151545634 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -31,15 +31,6 @@ RangeData TransformRangeData(const RangeData& range_data, }; } -TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, - const transform::Rigid3f& transform) { - return TimedRangeData{ - transform * range_data.origin, - TransformTimedPointCloud(range_data.returns, transform), - TransformTimedPointCloud(range_data.misses, transform), - }; -} - RangeData CropRangeData(const RangeData& range_data, const float min_z, const float max_z) { return RangeData{range_data.origin, @@ -47,13 +38,6 @@ RangeData CropRangeData(const RangeData& range_data, const float min_z, CropPointCloud(range_data.misses, min_z, max_z)}; } -TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, - const float min_z, const float max_z) { - return TimedRangeData{range_data.origin, - CropTimedPointCloud(range_data.returns, min_z, max_z), - CropTimedPointCloud(range_data.misses, min_z, max_z)}; -} - proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 7439bc7768..e7cd658d9b 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -35,26 +35,12 @@ struct RangeData { PointCloud misses; }; -// Like 'RangeData', but with 'TimedPointClouds'. -struct TimedRangeData { - Eigen::Vector3f origin; - TimedPointCloud returns; - TimedPointCloud misses; -}; - RangeData TransformRangeData(const RangeData& range_data, const transform::Rigid3f& transform); -TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, - const transform::Rigid3f& transform); - // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); -// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. -TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z, - float max_z); - // Converts 'range_data' to a proto::RangeData. proto::RangeData ToProto(const RangeData& range_data); From 98d957ae43f5b904cb99f0b1cdca657894336bef Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Fri, 21 Aug 2020 14:20:31 +0200 Subject: [PATCH 50/99] Remove legacy compatibility proto files. (#1736) Not needed anymore after the recent removal of that feature, see #1709. Signed-off-by: Michael Grupp --- cartographer/mapping/map_builder.cc | 1 - .../internal/legacy_probability_grid.proto | 37 --------------- .../internal/legacy_serialized_data.proto | 42 ----------------- .../proto/internal/legacy_submap.proto | 46 ------------------- 4 files changed, 126 deletions(-) delete mode 100644 cartographer/mapping/proto/internal/legacy_probability_grid.proto delete mode 100644 cartographer/mapping/proto/internal/legacy_serialized_data.proto delete mode 100644 cartographer/mapping/proto/internal/legacy_submap.proto diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index c93cd12cad..7809e6f262 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -28,7 +28,6 @@ #include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/collated_trajectory_builder.h" #include "cartographer/mapping/internal/global_trajectory_builder.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" #include "cartographer/sensor/internal/collator.h" #include "cartographer/sensor/internal/trajectory_collator.h" #include "cartographer/sensor/internal/voxel_filter.h" diff --git a/cartographer/mapping/proto/internal/legacy_probability_grid.proto b/cartographer/mapping/proto/internal/legacy_probability_grid.proto deleted file mode 100644 index 58d9d68ff4..0000000000 --- a/cartographer/mapping/proto/internal/legacy_probability_grid.proto +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2016 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. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/2d/map_limits.proto"; - -// The legacy probability grid that was used before the generalized -// grid structure was introduced. - -message LegacyProbabilityGrid { - message CellBox { - int32 max_x = 1; - int32 max_y = 2; - int32 min_x = 3; - int32 min_y = 4; - } - - MapLimits limits = 1; - // These values are actually int16s, but protos don't have a native int16 - // type. - repeated int32 cells = 2; - CellBox known_cells_box = 8; -} diff --git a/cartographer/mapping/proto/internal/legacy_serialized_data.proto b/cartographer/mapping/proto/internal/legacy_serialized_data.proto deleted file mode 100644 index 56f9c6a89f..0000000000 --- a/cartographer/mapping/proto/internal/legacy_serialized_data.proto +++ /dev/null @@ -1,42 +0,0 @@ -// 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. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/serialization.proto"; -import "cartographer/mapping/proto/internal/legacy_submap.proto"; - -message LegacySerializedData { - Submap submap = 1; - Node node = 2; - ImuData imu_data = 3; - OdometryData odometry_data = 4; - FixedFramePoseData fixed_frame_pose_data = 5; - TrajectoryData trajectory_data = 6; - LandmarkData landmark_data = 7; -} - -// For backwards compatibility with serialized data containing the legacy -// submap format that did not yet use the generalized 2D grid format. -message LegacySerializedDataLegacySubmap { - LegacySubmap submap = 1; - Node node = 2; - ImuData imu_data = 3; - OdometryData odometry_data = 4; - FixedFramePoseData fixed_frame_pose_data = 5; - TrajectoryData trajectory_data = 6; - LandmarkData landmark_data = 7; -} diff --git a/cartographer/mapping/proto/internal/legacy_submap.proto b/cartographer/mapping/proto/internal/legacy_submap.proto deleted file mode 100644 index 31ed41f888..0000000000 --- a/cartographer/mapping/proto/internal/legacy_submap.proto +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2016 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. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/pose_graph.proto"; -import "cartographer/mapping/proto/internal/legacy_probability_grid.proto"; -import "cartographer/mapping/proto/3d/hybrid_grid.proto"; -import "cartographer/transform/proto/transform.proto"; - -// Serialized state of a Submap2D. -// Uses the legacy, non-generalized probability grid format. -message LegacySubmap2D { - transform.proto.Rigid3d local_pose = 1; - int32 num_range_data = 2; - bool finished = 3; - LegacyProbabilityGrid probability_grid = 4; -} - -// Serialized state of a Submap3D. -message LegacySubmap3D { - transform.proto.Rigid3d local_pose = 1; - int32 num_range_data = 2; - bool finished = 3; - HybridGrid high_resolution_hybrid_grid = 4; - HybridGrid low_resolution_hybrid_grid = 5; -} - -message LegacySubmap { - SubmapId submap_id = 1; - LegacySubmap2D submap_2d = 2; - LegacySubmap3D submap_3d = 3; -} From 01cb6b1d6f2e67d0678d7ff2fe974d7ab956c49c Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 25 Aug 2020 11:04:59 +0200 Subject: [PATCH 51/99] Add gauge metric for current number of submap scan matchers. (#1738) Useful for analyzing memory-related issues. See: #1737 Signed-off-by: Michael Grupp --- .../mapping/internal/constraints/constraint_builder_2d.cc | 7 +++++++ .../mapping/internal/constraints/constraint_builder_3d.cc | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index 58e830c1f1..d612e7d2e3 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -48,6 +48,7 @@ static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); static auto* kQueueLengthMetric = metrics::Gauge::Null(); static auto* kConstraintScoresMetric = metrics::Histogram::Null(); static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) { return transform::Project2D(submap.local_pose()); @@ -163,6 +164,7 @@ ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, return &submap_scan_matchers_.at(submap_id); } auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); submap_scan_matcher.grid = grid; auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options(); auto scan_matcher_task = absl::make_unique(); @@ -303,6 +305,7 @@ void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { @@ -326,6 +329,10 @@ void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { "Constraint scores built", boundaries); kConstraintScoresMetric = scores->Add({{"search_region", "local"}}); kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); } } // namespace constraints diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index ef6dbd4edc..1672977e9b 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -54,6 +54,7 @@ static auto* kGlobalConstraintRotationalScoresMetric = metrics::Histogram::Null(); static auto* kGlobalConstraintLowResolutionScoresMetric = metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); ConstraintBuilder3D::ConstraintBuilder3D( const proto::ConstraintBuilderOptions& options, @@ -167,6 +168,7 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id, return &submap_scan_matchers_.at(submap_id); } auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); submap_scan_matcher.high_resolution_hybrid_grid = &submap->high_resolution_hybrid_grid(); submap_scan_matcher.low_resolution_hybrid_grid = @@ -334,6 +336,7 @@ void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { @@ -367,6 +370,10 @@ void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { scores->Add({{"search_region", "global"}, {"kind", "rotational_score"}}); kGlobalConstraintLowResolutionScoresMetric = scores->Add( {{"search_region", "global"}, {"kind", "low_resolution_score"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_3d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); } } // namespace constraints From da779339fa8b0ee28a4048b407df0ecf2fe65f0c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 26 Aug 2020 14:35:20 +0200 Subject: [PATCH 52/99] Add IntensityHybridGrid. (#1739) Adds a new structure IntensityHybridGrid, similar to HybridGrid but which stores intensities instead of probabilities. InterpolatedGrid is adapted to handle both types of HybridGrids. Signed-off-by: Wolfgang Hess --- cartographer/mapping/3d/hybrid_grid.h | 26 +++++++++++ cartographer/mapping/3d/hybrid_grid_test.cc | 17 +++++++ .../3d/scan_matching/interpolated_grid.h | 44 ++++++++++++------- .../scan_matching/interpolated_grid_test.cc | 13 +++--- .../occupied_space_cost_function_3d.h | 4 +- 5 files changed, 81 insertions(+), 23 deletions(-) diff --git a/cartographer/mapping/3d/hybrid_grid.h b/cartographer/mapping/3d/hybrid_grid.h index 10d2a5969a..709f279279 100644 --- a/cartographer/mapping/3d/hybrid_grid.h +++ b/cartographer/mapping/3d/hybrid_grid.h @@ -544,6 +544,32 @@ class HybridGrid : public HybridGridBase { std::vector update_indices_; }; +struct AverageIntensityData { + float sum = 0.f; + int count = 0; +}; + +class IntensityHybridGrid : public HybridGridBase { + public: + explicit IntensityHybridGrid(const float resolution) + : HybridGridBase(resolution) {} + + void AddIntensity(const Eigen::Array3i& index, const float intensity) { + AverageIntensityData* const cell = mutable_value(index); + cell->count += 1; + cell->sum += intensity; + } + + float GetIntensity(const Eigen::Array3i& index) const { + const AverageIntensityData& cell = value(index); + if (cell.count == 0) { + return 0.f; + } else { + return cell.sum / cell.count; + } + } +}; + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/3d/hybrid_grid_test.cc b/cartographer/mapping/3d/hybrid_grid_test.cc index 333b114bbd..f600032e36 100644 --- a/cartographer/mapping/3d/hybrid_grid_test.cc +++ b/cartographer/mapping/3d/hybrid_grid_test.cc @@ -84,6 +84,23 @@ TEST(HybridGridTest, GetProbability) { } } +TEST(HybridGridTest, GetIntensity) { + IntensityHybridGrid hybrid_grid(1.f); + const Eigen::Array3i cell_index = + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)); + const float intensity = 58.0f; + + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9); + hybrid_grid.AddIntensity(cell_index, intensity); + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { + EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9); + } +} + MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } TEST(HybridGridTest, GetCellIndex) { diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h index d2b30d70fe..b38fdbe963 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h @@ -25,22 +25,23 @@ namespace cartographer { namespace mapping { namespace scan_matching { -// Interpolates between HybridGrid probability voxels. We use the tricubic +// Interpolates between HybridGrid voxels. We use the tricubic // interpolation which interpolates the values and has vanishing derivative at // these points. // // This class is templated to work with the autodiff that Ceres provides. // For this reason, it is also important that the interpolation scheme be // continuously differentiable. +template class InterpolatedGrid { public: - explicit InterpolatedGrid(const HybridGrid& hybrid_grid) + explicit InterpolatedGrid(const HybridGridType& hybrid_grid) : hybrid_grid_(hybrid_grid) {} - InterpolatedGrid(const InterpolatedGrid&) = delete; - InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; + InterpolatedGrid(const InterpolatedGrid&) = delete; + InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; - // Returns the interpolated probability at (x, y, z) of the HybridGrid + // Returns the interpolated value at (x, y, z) of the HybridGrid // used to perform the interpolation. // // This is a piecewise, continuously differentiable function. We use the @@ -48,27 +49,27 @@ class InterpolatedGrid { // tensor product volume of piecewise cubic polynomials that interpolate // the values, and have vanishing derivative at the interval boundaries. template - T GetProbability(const T& x, const T& y, const T& z) const { + T GetInterpolatedValue(const T& x, const T& y, const T& z) const { double x1, y1, z1, x2, y2, z2; ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); const Eigen::Array3i index1 = hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); - const double q111 = hybrid_grid_.GetProbability(index1); + const double q111 = GetValue(hybrid_grid_, index1); const double q112 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1)); const double q121 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0)); const double q122 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1)); const double q211 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0)); const double q212 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1)); const double q221 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0)); const double q222 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1)); const T normalized_x = (x - x1) / (x2 - x1); const T normalized_y = (y - y1) / (y2 - y1); @@ -145,9 +146,22 @@ class InterpolatedGrid { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } - const HybridGrid& hybrid_grid_; + static float GetValue(const HybridGrid& probability_grid, + const Eigen::Array3i& index) { + return probability_grid.GetProbability(index); + } + + static float GetValue(const IntensityHybridGrid& intensity_grid, + const Eigen::Array3i& index) { + return intensity_grid.GetIntensity(index); + } + + const HybridGridType& hybrid_grid_; }; +using InterpolatedIntensityGrid = InterpolatedGrid; +using InterpolatedProbabilityGrid = InterpolatedGrid; + } // namespace scan_matching } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc index f11126da42..503b2ecf11 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc @@ -44,7 +44,7 @@ class InterpolatedGridTest : public ::testing::Test { } HybridGrid hybrid_grid_; - InterpolatedGrid interpolated_grid_; + InterpolatedProbabilityGrid interpolated_grid_; }; TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { @@ -52,7 +52,7 @@ TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { EXPECT_NEAR(GetHybridGridProbability(x, y, z), - interpolated_grid_.GetProbability(x, y, z), 1e-6); + interpolated_grid_.GetInterpolatedValue(x, y, z), 1e-6); } } } @@ -73,10 +73,11 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { for (double sample = kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample += kSampleStep) { - EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( - x + sample + kSampleStep, y, z) - - interpolated_grid_.GetProbability( - x + sample, y, z))); + EXPECT_LT(0., + grid_difference * (interpolated_grid_.GetInterpolatedValue( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetInterpolatedValue( + x + sample, y, z))); } } } diff --git a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h index 5e0cb807e2..ff5b65373b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h @@ -73,7 +73,7 @@ class OccupiedSpaceCostFunction3D { const Eigen::Matrix world = transform * point_cloud_[i].position.cast(); const T probability = - interpolated_grid_.GetProbability(world[0], world[1], world[2]); + interpolated_grid_.GetInterpolatedValue(world[0], world[1], world[2]); residual[i] = scaling_factor_ * (1. - probability); } return true; @@ -81,7 +81,7 @@ class OccupiedSpaceCostFunction3D { const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const InterpolatedGrid interpolated_grid_; + const InterpolatedProbabilityGrid interpolated_grid_; }; } // namespace scan_matching From a20db758cdce822528a90d4812af6887f495c35e Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 28 Aug 2020 10:12:04 +0200 Subject: [PATCH 53/99] Add intensity data to TimedPointCloudData. (#1742) Adds a new field intensities to TimedPointCloudData. RangeDataCollator now also takes intensities into account. AddRangeData now takes a point cloud by value instead of const reference as we would later make a copy of it anyway. Signed-off-by: Wolfgang Hess --- .../mapping/internal/range_data_collator.cc | 28 +++++--- .../mapping/internal/range_data_collator.h | 6 +- .../internal/range_data_collator_test.cc | 71 ++++++++++++++----- cartographer/sensor/proto/sensor.proto | 1 + cartographer/sensor/timed_point_cloud_data.cc | 9 ++- cartographer/sensor/timed_point_cloud_data.h | 3 + 6 files changed, 90 insertions(+), 28 deletions(-) diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 702565afcf..6c27ac3d67 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -25,10 +25,14 @@ namespace cartographer { namespace mapping { +constexpr float RangeDataCollator::kDefaultIntensityValue; + sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data) { + sensor::TimedPointCloudData timed_point_cloud_data) { CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); + timed_point_cloud_data.intensities.resize( + timed_point_cloud_data.ranges.size(), kDefaultIntensityValue); // TODO(gaschler): These two cases can probably be one. if (id_to_pending_data_.count(sensor_id) != 0) { current_start_ = current_end_; @@ -36,10 +40,10 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( // the two (do not send out current). current_end_ = id_to_pending_data_.at(sensor_id).time; auto result = CropAndMerge(); - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); return result; } - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { return {}; } @@ -59,7 +63,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { for (auto it = id_to_pending_data_.begin(); it != id_to_pending_data_.end();) { sensor::TimedPointCloudData& data = it->second; - sensor::TimedPointCloud& ranges = it->second.ranges; + const sensor::TimedPointCloud& ranges = it->second.ranges; + const std::vector& intensities = it->second.intensities; auto overlap_begin = ranges.begin(); while (overlap_begin < ranges.end() && @@ -85,10 +90,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { result.origins.push_back(data.origin); const float time_correction = static_cast(common::ToSeconds(data.time - current_end_)); + auto intensities_overlap_it = + intensities.begin() + (overlap_begin - ranges.begin()); + result.ranges.reserve(result.ranges.size() + + std::distance(overlap_begin, overlap_end)); for (auto overlap_it = overlap_begin; overlap_it != overlap_end; - ++overlap_it) { - sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it, - origin_index}; + ++overlap_it, ++intensities_overlap_it) { + sensor::TimedPointCloudOriginData::RangeMeasurement point{ + *overlap_it, *intensities_overlap_it, origin_index}; // current_end_ + point_time[3]_after == in_timestamp + // point_time[3]_before point.point_time.time += time_correction; @@ -102,9 +111,12 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { } else if (overlap_end == ranges.begin()) { ++it; } else { + const auto intensities_overlap_end = + intensities.begin() + (overlap_end - ranges.begin()); data = sensor::TimedPointCloudData{ data.time, data.origin, - sensor::TimedPointCloud(overlap_end, ranges.end())}; + sensor::TimedPointCloud(overlap_end, ranges.end()), + std::vector(intensities_overlap_end, intensities.end())}; ++it; } } diff --git a/cartographer/mapping/internal/range_data_collator.h b/cartographer/mapping/internal/range_data_collator.h index 8328828df4..29cf1381ea 100644 --- a/cartographer/mapping/internal/range_data_collator.h +++ b/cartographer/mapping/internal/range_data_collator.h @@ -37,9 +37,11 @@ class RangeDataCollator { : expected_sensor_ids_(expected_range_sensor_ids.begin(), expected_range_sensor_ids.end()) {} + // If timed_point_cloud_data has incomplete intensity data, we will fill the + // missing intensities with kDefaultIntensityValue. sensor::TimedPointCloudOriginData AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data); + sensor::TimedPointCloudData timed_point_cloud_data); private: sensor::TimedPointCloudOriginData CropAndMerge(); @@ -49,6 +51,8 @@ class RangeDataCollator { std::map id_to_pending_data_; common::Time current_start_ = common::Time::min(); common::Time current_end_ = common::Time::min(); + + constexpr static float kDefaultIntensityValue = 0.f; }; } // namespace mapping diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc index 8799c19c7a..43ceb00a9d 100644 --- a/cartographer/mapping/internal/range_data_collator_test.cc +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -26,16 +26,21 @@ namespace { const int kNumSamples = 10; -sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) { +sensor::TimedPointCloudData CreateFakeRangeData(int from, int to, + bool fake_intensities) { double duration = common::ToSeconds(common::FromUniversal(to) - common::FromUniversal(from)); sensor::TimedPointCloudData result{ - common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}}; + common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}}; result.ranges.reserve(kNumSamples); for (int i = 0; i < kNumSamples; ++i) { double fraction = static_cast(i) / (kNumSamples - 1); - float relative_time = (1.f - fraction) * -duration; - result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time}); + float relative_time = (1. - fraction) * -duration; + result.ranges.push_back( + {Eigen::Vector3f{1., 2., static_cast(fraction)}, relative_time}); + if (fake_intensities) { + result.intensities.push_back(result.ranges.back().position.z()); + } } return result; } @@ -49,17 +54,23 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) { return std::is_sorted(timestamps.begin(), timestamps.end()); } +void IntensitiesAreConsistent(const sensor::TimedPointCloudOriginData& data) { + for (const auto& range : data.ranges) { + EXPECT_NEAR(range.point_time.position.z(), range.intensity, 1e-6); + } +} + TEST(RangeDataCollatorTest, SingleSensor) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); auto output_0 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(common::ToUniversal(output_0.time), 300); EXPECT_EQ(output_0.origins.size(), 1); EXPECT_EQ(output_0.ranges.size(), kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_0)); auto output_1 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -69,7 +80,7 @@ TEST(RangeDataCollatorTest, SingleSensor) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -80,13 +91,14 @@ TEST(RangeDataCollatorTest, SingleSensor) { TEST(RangeDataCollatorTest, SingleSensorEmptyData) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); - sensor::TimedPointCloudData empty_data{common::FromUniversal(300)}; + sensor::TimedPointCloudData empty_data{ + common::FromUniversal(300), {}, {}, {}}; auto output_0 = collator.AddRangeData(sensor_id, empty_data); EXPECT_EQ(output_0.time, empty_data.time); EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size()); EXPECT_TRUE(ArePointTimestampsSorted(output_0)); auto output_1 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -96,7 +108,7 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -109,10 +121,10 @@ TEST(RangeDataCollatorTest, TwoSensors) { const std::string sensor_1 = "sensor_1"; RangeDataCollator collator({sensor_0, sensor_1}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310, false)); EXPECT_EQ(output_1.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_1.time), 300); ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); @@ -123,7 +135,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_1)); auto output_2 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(output_2.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_2.time), 310); ASSERT_EQ(output_2.ranges.size(), 2); @@ -135,7 +147,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_TRUE(ArePointTimestampsSorted(output_2)); // Sending the same sensor will flush everything before. auto output_3 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700, false)); EXPECT_EQ(common::ToUniversal(output_3.time), 500); EXPECT_EQ( output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), @@ -150,19 +162,42 @@ TEST(RangeDataCollatorTest, ThreeSensors) { const std::string sensor_2 = "sensor_2"; RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); +} + +TEST(RangeDataCollatorTest, ThreeSensorsWithIntensities) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + const std::string sensor_2 = "sensor_2"; + RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, true)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, true)); EXPECT_EQ(output_1.ranges.size(), 0); auto output_2 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, true)); EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + IntensitiesAreConsistent(output_2); auto output_3 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, true)); EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_3)); + IntensitiesAreConsistent(output_3); } } // namespace diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 0b445b3ca5..62cc640a93 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -41,6 +41,7 @@ message TimedPointCloudData { transform.proto.Vector3f origin = 2; repeated transform.proto.Vector4f point_data_legacy = 3; repeated TimedRangefinderPoint point_data = 4; + repeated float intensities = 5; } // Proto representation of ::cartographer::sensor::RangeData. diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc index 63a17af397..2f01f08a4e 100644 --- a/cartographer/sensor/timed_point_cloud_data.cc +++ b/cartographer/sensor/timed_point_cloud_data.cc @@ -31,10 +31,15 @@ proto::TimedPointCloudData ToProto( for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { *proto.add_point_data() = ToProto(range); } + for (const float intensity : timed_point_cloud_data.intensities) { + proto.add_intensities(intensity); + } return proto; } TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { + CHECK(proto.intensities().size() == 0 || + proto.intensities().size() == proto.point_data().size()); TimedPointCloud timed_point_cloud; if (proto.point_data().size() > 0) { timed_point_cloud.reserve(proto.point_data().size()); @@ -50,7 +55,9 @@ TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { } return TimedPointCloudData{common::FromUniversal(proto.timestamp()), transform::ToEigen(proto.origin()), - timed_point_cloud}; + timed_point_cloud, + std::vector(proto.intensities().begin(), + proto.intensities().end())}; } } // namespace sensor diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 4901e3587b..8bdd928027 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -28,11 +28,14 @@ struct TimedPointCloudData { common::Time time; Eigen::Vector3f origin; TimedPointCloud ranges; + // 'intensities' has to be same size as 'ranges', or empty. + std::vector intensities; }; struct TimedPointCloudOriginData { struct RangeMeasurement { TimedRangefinderPoint point_time; + float intensity; size_t origin_index; }; common::Time time; From a28e24a37da73c709517f90ba4eb6fcb8c722f43 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 28 Aug 2020 11:23:50 +0200 Subject: [PATCH 54/99] Remove unused and untested CropTimedPointCloud. (#1744) Signed-off-by: Wolfgang Hess --- cartographer/sensor/point_cloud.cc | 11 ----------- cartographer/sensor/point_cloud.h | 5 ----- 2 files changed, 16 deletions(-) diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 1d5652027c..959017c424 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -53,16 +53,5 @@ PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, return cropped_point_cloud; } -TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, - const float min_z, const float max_z) { - TimedPointCloud cropped_point_cloud; - for (const TimedRangefinderPoint& point : point_cloud) { - if (min_z <= point.position.z() && point.position.z() <= max_z) { - cropped_point_cloud.push_back(point); - } - } - return cropped_point_cloud; -} - } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index dd96965a98..d2c338d393 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -57,11 +57,6 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z, float max_z); -// Returns a new point cloud without points that fall outside the region defined -// by 'min_z' and 'max_z'. -TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, - float min_z, float max_z); - } // namespace sensor } // namespace cartographer From 2137b8477e2e42a1615ca34d7e4a7af69feb65fe Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Mon, 31 Aug 2020 02:23:47 -0700 Subject: [PATCH 55/99] Updated the ROS on Windows feed URL (#1743) Signed-off-by: seanyen --- azure-pipelines.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/azure-pipelines.yml b/azure-pipelines.yml index 096ed61ff5..cd6c908d0e 100644 --- a/azure-pipelines.yml +++ b/azure-pipelines.yml @@ -19,7 +19,7 @@ jobs: timeoutInMinutes: 360 steps: - script: | - choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1 + choco sources add -n=roswin -s https://aka.ms/ros/public --priority 1 rem Azure VM runs out of space on C:, so use D: for ros and rosdeps mkdir D:\opt && mklink /J C:\opt D:\opt choco upgrade %ROS_METAPACKAGE% -y From f2035b087759a6b085290fcd8ff39eadf20f2db0 Mon Sep 17 00:00:00 2001 From: Yannic Date: Mon, 14 Sep 2020 11:42:59 +0200 Subject: [PATCH 56/99] Upgrade Protobuf to v3.13.0 (#1746) This makes cartographer compatible with `--incompatible_blacklisted_protos_requires_proto_info`. See https://github.com/bazelbuild/bazel/issues/11694 Signed-off-by: Yannic Bonenberger --- bazel/repositories.bzl | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index c24bc91f38..c4bd67eb0f 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -206,19 +206,21 @@ def cartographer_repositories(): _maybe( http_archive, name = "bazel_skylib", - strip_prefix = "bazel-skylib-67215655bf6ce349b2b88ae4f5945a706f8ce959", - urls = ["https://github.com/bazelbuild/bazel-skylib/archive/67215655bf6ce349b2b88ae4f5945a706f8ce959.tar.gz"], + sha256 = "e5d90f0ec952883d56747b7604e2a15ee36e288bb556c3d0ed33e818a4d971f2", + strip_prefix = "bazel-skylib-1.0.2", + urls = ["https://github.com/bazelbuild/bazel-skylib/archive/1.0.2.tar.gz"], ) _maybe( http_archive, name = "com_google_protobuf", - sha256 = "f1748989842b46fa208b2a6e4e2785133cfcc3e4d43c17fecb023733f0f5443f", - strip_prefix = "protobuf-3.7.1", + sha256 = "1c744a6a1f2c901e68c5521bc275e22bdc66256eeb605c2781923365b7087e5f", + strip_prefix = "protobuf-3.13.0", urls = [ - "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.7.1.tar.gz", - "https://github.com/google/protobuf/archive/v3.7.1.tar.gz", + "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.13.0.zip", + "https://github.com/google/protobuf/archive/v3.13.0.zip", ], + repo_mapping = {"@zlib": "@net_zlib_zlib"}, ) _maybe( @@ -272,6 +274,17 @@ def cartographer_repositories(): urls = ["https://github.com/abseil/abseil-cpp/archive/5441bbe1db5d0f2ca24b5b60166367b0966790af.tar.gz"], ) + _maybe( + http_archive, + name = "rules_python", + sha256 = "e5470e92a18aa51830db99a4d9c492cc613761d5bdb7131c04bd92b9834380f6", + strip_prefix = "rules_python-4b84ad270387a7c439ebdccfd530e2339601ef27", + urls = [ + "https://mirror.bazel.build/github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + "https://github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + ], + ) + # TODO(rodrigoq): remove these binds once grpc#14140 has been merged, as well # as removing `use_external` in cartographer_grpc/BUILD.bazel. # https://github.com/grpc/grpc/pull/14140 From c5416068dd06ac4ca732ed53db225204b25ec05b Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 14 Sep 2020 12:17:15 +0200 Subject: [PATCH 57/99] Move proto files out of 2d/3d directories. (#1749) This is to make it possible to use the proto definitions from Python where the directory structure otherwise prevents them from being imported in the usual way. Signed-off-by: Wolfgang Hess --- cartographer/io/hybrid_grid_points_processor.h | 2 +- cartographer/io/probability_grid_points_processor.h | 2 +- cartographer/mapping/2d/grid_2d.h | 4 ++-- cartographer/mapping/2d/map_limits.h | 2 +- .../mapping/2d/probability_grid_range_data_inserter_2d.h | 2 +- cartographer/mapping/2d/submap_2d.h | 2 +- cartographer/mapping/2d/tsdf_range_data_inserter_2d.h | 4 ++-- cartographer/mapping/2d/xy_index.h | 2 +- cartographer/mapping/3d/hybrid_grid.h | 2 +- cartographer/mapping/3d/range_data_inserter_3d.h | 2 +- cartographer/mapping/3d/submap_3d.h | 2 +- .../mapping/internal/2d/local_trajectory_builder_2d.h | 2 +- .../internal/2d/local_trajectory_builder_options_2d.h | 2 +- cartographer/mapping/internal/2d/normal_estimation_2d.h | 2 +- .../mapping/internal/3d/local_trajectory_builder_3d.cc | 8 ++++---- .../mapping/internal/3d/local_trajectory_builder_3d.h | 2 +- .../internal/3d/local_trajectory_builder_options_3d.h | 2 +- .../3d/scan_matching/fast_correlative_scan_matcher_3d.cc | 2 +- .../mapping/internal/constraints/constraint_builder_2d.cc | 4 ++-- .../mapping/internal/constraints/constraint_builder_3d.cc | 4 ++-- .../proto/{2d/cell_limits.proto => cell_limits_2d.proto} | 0 cartographer/mapping/proto/{2d => }/grid_2d.proto | 6 +++--- cartographer/mapping/proto/{2d => }/grid_2d_options.proto | 0 cartographer/mapping/proto/{3d => }/hybrid_grid.proto | 0 .../{2d => }/local_trajectory_builder_options_2d.proto | 2 +- .../{3d => }/local_trajectory_builder_options_3d.proto | 2 +- cartographer/mapping/proto/{2d => }/map_limits.proto | 2 +- .../proto/{2d => }/normal_estimation_options_2d.proto | 0 .../mapping/proto/{2d => }/probability_grid.proto | 0 .../probability_grid_range_data_inserter_options_2d.proto | 0 .../mapping/proto/range_data_inserter_options.proto | 4 ++-- .../proto/{3d => }/range_data_inserter_options_3d.proto | 0 cartographer/mapping/proto/submap.proto | 4 ++-- .../mapping/proto/{2d => }/submaps_options_2d.proto | 2 +- .../mapping/proto/{3d => }/submaps_options_3d.proto | 2 +- .../mapping/proto/trajectory_builder_options.proto | 4 ++-- cartographer/mapping/proto/{2d => }/tsdf_2d.proto | 0 .../{2d => }/tsdf_range_data_inserter_options_2d.proto | 2 +- cartographer/mapping/range_data_inserter_interface.h | 2 +- 39 files changed, 43 insertions(+), 43 deletions(-) rename cartographer/mapping/proto/{2d/cell_limits.proto => cell_limits_2d.proto} (100%) rename cartographer/mapping/proto/{2d => }/grid_2d.proto (86%) rename cartographer/mapping/proto/{2d => }/grid_2d_options.proto (100%) rename cartographer/mapping/proto/{3d => }/hybrid_grid.proto (100%) rename cartographer/mapping/proto/{2d => }/local_trajectory_builder_options_2d.proto (98%) rename cartographer/mapping/proto/{3d => }/local_trajectory_builder_options_3d.proto (98%) rename cartographer/mapping/proto/{2d => }/map_limits.proto (93%) rename cartographer/mapping/proto/{2d => }/normal_estimation_options_2d.proto (100%) rename cartographer/mapping/proto/{2d => }/probability_grid.proto (100%) rename cartographer/mapping/proto/{2d => }/probability_grid_range_data_inserter_options_2d.proto (100%) rename cartographer/mapping/proto/{3d => }/range_data_inserter_options_3d.proto (100%) rename cartographer/mapping/proto/{2d => }/submaps_options_2d.proto (94%) rename cartographer/mapping/proto/{3d => }/submaps_options_3d.proto (94%) rename cartographer/mapping/proto/{2d => }/tsdf_2d.proto (100%) rename cartographer/mapping/proto/{2d => }/tsdf_range_data_inserter_options_2d.proto (95%) diff --git a/cartographer/io/hybrid_grid_points_processor.h b/cartographer/io/hybrid_grid_points_processor.h index 0b598b92d8..8124e4087f 100644 --- a/cartographer/io/hybrid_grid_points_processor.h +++ b/cartographer/io/hybrid_grid_points_processor.h @@ -11,7 +11,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" -#include "cartographer/mapping/proto/3d/range_data_inserter_options_3d.pb.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" namespace cartographer { namespace io { diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index fe7e74f77c..2114bf8d0c 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -25,7 +25,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping/value_conversion_tables.h" diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 527c456487..dfea1e5fee 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -22,9 +22,9 @@ #include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/grid_interface.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/2d/grid_2d.pb.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" +#include "cartographer/mapping/proto/grid_2d.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/mapping/value_conversion_tables.h" namespace cartographer { diff --git a/cartographer/mapping/2d/map_limits.h b/cartographer/mapping/2d/map_limits.h index 241fc570a7..0ad1f3c88f 100644 --- a/cartographer/mapping/2d/map_limits.h +++ b/cartographer/mapping/2d/map_limits.h @@ -24,7 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/xy_index.h" -#include "cartographer/mapping/proto/2d/map_limits.pb.h" +#include "cartographer/mapping/proto/map_limits.pb.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h index 83a04b8971..6271094213 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h @@ -24,7 +24,7 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/xy_index.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index 8ea9097718..da30cbbe2d 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -24,9 +24,9 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h index 61a77a2f58..a3fa5ba775 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h @@ -19,7 +19,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" @@ -57,4 +57,4 @@ class TSDFRangeDataInserter2D : public RangeDataInserterInterface { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ \ No newline at end of file +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ diff --git a/cartographer/mapping/2d/xy_index.h b/cartographer/mapping/2d/xy_index.h index 64acf274c2..2899e2dacc 100644 --- a/cartographer/mapping/2d/xy_index.h +++ b/cartographer/mapping/2d/xy_index.h @@ -25,7 +25,7 @@ #include "Eigen/Core" #include "cartographer/common/math.h" #include "cartographer/common/port.h" -#include "cartographer/mapping/proto/2d/cell_limits.pb.h" +#include "cartographer/mapping/proto/cell_limits_2d.pb.h" #include "glog/logging.h" namespace cartographer { diff --git a/cartographer/mapping/3d/hybrid_grid.h b/cartographer/mapping/3d/hybrid_grid.h index 709f279279..5b746b5eaf 100644 --- a/cartographer/mapping/3d/hybrid_grid.h +++ b/cartographer/mapping/3d/hybrid_grid.h @@ -28,7 +28,7 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/3d/hybrid_grid.pb.h" +#include "cartographer/mapping/proto/hybrid_grid.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/3d/range_data_inserter_3d.h b/cartographer/mapping/3d/range_data_inserter_3d.h index b6b0bcd0b5..d21d2fac5e 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.h +++ b/cartographer/mapping/3d/range_data_inserter_3d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ #include "cartographer/mapping/3d/hybrid_grid.h" -#include "cartographer/mapping/proto/3d/range_data_inserter_options_3d.pb.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index b89da0f495..5be68cb61e 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -26,9 +26,9 @@ #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h index 1a9346378d..faed1df105 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h @@ -27,7 +27,7 @@ #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" #include "cartographer/mapping/pose_extrapolator.h" -#include "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/internal/voxel_filter.h" diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h index 32fa74228e..768492060a 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.h b/cartographer/mapping/internal/2d/normal_estimation_2d.h index 61e0a2f092..9a344261fb 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d.h +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.h @@ -19,7 +19,7 @@ #include -#include "cartographer/mapping/proto/2d/normal_estimation_options_2d.pb.h" +#include "cartographer/mapping/proto/normal_estimation_options_2d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 07c5aec2b4..773dbcece1 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -21,10 +21,10 @@ #include "absl/memory/memory.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" -#include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" #include "cartographer/transform/timestamped_transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 69f034eed9..f242c385af 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -27,7 +27,7 @@ #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" #include "cartographer/mapping/pose_extrapolator_interface.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/internal/voxel_filter.h" diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h index b01b298b5b..e26a6ec0e4 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc index da4bb91e77..0a0a895382 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -25,7 +25,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index d612e7d2e3..dcdb1d1120 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -29,8 +29,8 @@ #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_2d.pb.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h" #include "cartographer/metrics/counter.h" #include "cartographer/metrics/gauge.h" #include "cartographer/metrics/histogram.h" diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index 1672977e9b..878aaa032f 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -29,8 +29,8 @@ #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/metrics/counter.h" #include "cartographer/metrics/gauge.h" #include "cartographer/metrics/histogram.h" diff --git a/cartographer/mapping/proto/2d/cell_limits.proto b/cartographer/mapping/proto/cell_limits_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/cell_limits.proto rename to cartographer/mapping/proto/cell_limits_2d.proto diff --git a/cartographer/mapping/proto/2d/grid_2d.proto b/cartographer/mapping/proto/grid_2d.proto similarity index 86% rename from cartographer/mapping/proto/2d/grid_2d.proto rename to cartographer/mapping/proto/grid_2d.proto index 70b246d25f..010d5b6081 100644 --- a/cartographer/mapping/proto/2d/grid_2d.proto +++ b/cartographer/mapping/proto/grid_2d.proto @@ -16,9 +16,9 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/map_limits.proto"; -import "cartographer/mapping/proto/2d/probability_grid.proto"; -import "cartographer/mapping/proto/2d/tsdf_2d.proto"; +import "cartographer/mapping/proto/map_limits.proto"; +import "cartographer/mapping/proto/probability_grid.proto"; +import "cartographer/mapping/proto/tsdf_2d.proto"; message Grid2D { message CellBox { diff --git a/cartographer/mapping/proto/2d/grid_2d_options.proto b/cartographer/mapping/proto/grid_2d_options.proto similarity index 100% rename from cartographer/mapping/proto/2d/grid_2d_options.proto rename to cartographer/mapping/proto/grid_2d_options.proto diff --git a/cartographer/mapping/proto/3d/hybrid_grid.proto b/cartographer/mapping/proto/hybrid_grid.proto similarity index 100% rename from cartographer/mapping/proto/3d/hybrid_grid.proto rename to cartographer/mapping/proto/hybrid_grid.proto diff --git a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto similarity index 98% rename from cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto rename to cartographer/mapping/proto/local_trajectory_builder_options_2d.proto index 470dfe81c6..6add120384 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto @@ -19,9 +19,9 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; -import "cartographer/mapping/proto/2d/submaps_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_2d.proto"; // NEXT ID: 22 message LocalTrajectoryBuilderOptions2D { diff --git a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto similarity index 98% rename from cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto rename to cartographer/mapping/proto/local_trajectory_builder_options_3d.proto index 7039ddf817..f29debd996 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto @@ -16,11 +16,11 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/3d/submaps_options_3d.proto"; import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_3d.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/sensor/proto/sensor.proto"; import "cartographer/transform/proto/timestamped_transform.proto"; diff --git a/cartographer/mapping/proto/2d/map_limits.proto b/cartographer/mapping/proto/map_limits.proto similarity index 93% rename from cartographer/mapping/proto/2d/map_limits.proto rename to cartographer/mapping/proto/map_limits.proto index 56f0a99076..53044e6a75 100644 --- a/cartographer/mapping/proto/2d/map_limits.proto +++ b/cartographer/mapping/proto/map_limits.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/cell_limits.proto"; +import "cartographer/mapping/proto/cell_limits_2d.proto"; import "cartographer/transform/proto/transform.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/2d/normal_estimation_options_2d.proto b/cartographer/mapping/proto/normal_estimation_options_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/normal_estimation_options_2d.proto rename to cartographer/mapping/proto/normal_estimation_options_2d.proto diff --git a/cartographer/mapping/proto/2d/probability_grid.proto b/cartographer/mapping/proto/probability_grid.proto similarity index 100% rename from cartographer/mapping/proto/2d/probability_grid.proto rename to cartographer/mapping/proto/probability_grid.proto diff --git a/cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto b/cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto rename to cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto diff --git a/cartographer/mapping/proto/range_data_inserter_options.proto b/cartographer/mapping/proto/range_data_inserter_options.proto index 7b0d5b1049..9470f2c8f2 100644 --- a/cartographer/mapping/proto/range_data_inserter_options.proto +++ b/cartographer/mapping/proto/range_data_inserter_options.proto @@ -14,8 +14,8 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto"; -import "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto b/cartographer/mapping/proto/range_data_inserter_options_3d.proto similarity index 100% rename from cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto rename to cartographer/mapping/proto/range_data_inserter_options_3d.proto diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 72b4e627c9..50331278cd 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -16,8 +16,8 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/grid_2d.proto"; -import "cartographer/mapping/proto/3d/hybrid_grid.proto"; +import "cartographer/mapping/proto/grid_2d.proto"; +import "cartographer/mapping/proto/hybrid_grid.proto"; import "cartographer/transform/proto/transform.proto"; // Serialized state of a Submap2D. diff --git a/cartographer/mapping/proto/2d/submaps_options_2d.proto b/cartographer/mapping/proto/submaps_options_2d.proto similarity index 94% rename from cartographer/mapping/proto/2d/submaps_options_2d.proto rename to cartographer/mapping/proto/submaps_options_2d.proto index ebb3c58900..65838da82a 100644 --- a/cartographer/mapping/proto/2d/submaps_options_2d.proto +++ b/cartographer/mapping/proto/submaps_options_2d.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/grid_2d_options.proto"; +import "cartographer/mapping/proto/grid_2d_options.proto"; import "cartographer/mapping/proto/range_data_inserter_options.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/3d/submaps_options_3d.proto b/cartographer/mapping/proto/submaps_options_3d.proto similarity index 94% rename from cartographer/mapping/proto/3d/submaps_options_3d.proto rename to cartographer/mapping/proto/submaps_options_3d.proto index 29b4db8665..822c6d9ee1 100644 --- a/cartographer/mapping/proto/3d/submaps_options_3d.proto +++ b/cartographer/mapping/proto/submaps_options_3d.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto"; +import "cartographer/mapping/proto/range_data_inserter_options_3d.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index d6d9c2f8dc..fe20db05d3 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -15,8 +15,8 @@ syntax = "proto3"; import "cartographer/transform/proto/transform.proto"; -import "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto"; -import "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/2d/tsdf_2d.proto b/cartographer/mapping/proto/tsdf_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/tsdf_2d.proto rename to cartographer/mapping/proto/tsdf_2d.proto diff --git a/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto b/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto similarity index 95% rename from cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto rename to cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto index 1b61f15d0c..d3ef1d3260 100644 --- a/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto +++ b/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto @@ -16,7 +16,7 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/normal_estimation_options_2d.proto"; +import "cartographer/mapping/proto/normal_estimation_options_2d.proto"; message TSDFRangeDataInserterOptions2D { // Distance to the surface within the signed distance function is evaluated. diff --git a/cartographer/mapping/range_data_inserter_interface.h b/cartographer/mapping/range_data_inserter_interface.h index 5170766d2a..3f3a20e464 100644 --- a/cartographer/mapping/range_data_inserter_interface.h +++ b/cartographer/mapping/range_data_inserter_interface.h @@ -21,7 +21,7 @@ #include #include "cartographer/mapping/grid_interface.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/sensor/range_data.h" namespace cartographer { From 1ad6398bce040fd1a1678cf08e506661aa376d6e Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 24 Sep 2020 11:32:40 +0200 Subject: [PATCH 58/99] Depend on libceres-dev in package.xml for ROS. (#1750) This changes cartographer_ros to no longer build Ceres and instead uses libceres-dev for which a suitable version is provided by ROS for Kinetic. Related to #1705. Signed-off-by: Wolfgang Hess --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index a8819cbde6..19c811cb09 100644 --- a/package.xml +++ b/package.xml @@ -41,9 +41,9 @@ python3-sphinx boost - ceres-solver eigen libcairo2-dev + libceres-dev libgflags-dev libgoogle-glog-dev lua5.2-dev From c84da8ec0f69a4dae2c9dddb2f01db0041431855 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 30 Sep 2020 18:32:39 +0200 Subject: [PATCH 59/99] Random voxel filtering. (#1753) This leads to slightly slower performance but better quality. Also replaces the VoxelFilter class by free standing functions. Signed-off-by: Wolfgang Hess --- .../2d/local_trajectory_builder_2d.cc | 8 +- .../3d/local_trajectory_builder_3d.cc | 26 ++-- cartographer/sensor/internal/voxel_filter.cc | 117 ++++++++++-------- cartographer/sensor/internal/voxel_filter.h | 56 ++------- .../sensor/internal/voxel_filter_test.cc | 41 +++--- 5 files changed, 117 insertions(+), 131 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 55a1e4f800..5bda7938a5 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -58,8 +58,8 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns), - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)}; + sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), + sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())}; } std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( @@ -226,8 +226,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); const sensor::PointCloud& filtered_gravity_aligned_point_cloud = - sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options()) - .Filter(gravity_aligned_range_data.returns); + sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns, + options_.adaptive_voxel_filter_options()); if (filtered_gravity_aligned_point_cloud.empty()) { return nullptr; } diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 773dbcece1..361f724cf0 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -154,9 +154,8 @@ LocalTrajectoryBuilder3D::AddRangeData( accumulated_point_cloud_origin_data_.clear(); } - synchronized_data.ranges = - sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) - .Filter(synchronized_data.ranges); + synchronized_data.ranges = sensor::VoxelFilter( + synchronized_data.ranges, 0.5f * options_.voxel_filter_size()); accumulated_point_cloud_origin_data_.emplace_back( std::move(synchronized_data)); ++num_accumulated_; @@ -237,10 +236,10 @@ LocalTrajectoryBuilder3D::AddRangeData( const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { extrapolation_result.current_pose.translation().cast(), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data.returns), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data.misses)}; + sensor::VoxelFilter(accumulated_range_data.returns, + options_.voxel_filter_size()), + sensor::VoxelFilter(accumulated_range_data.misses, + options_.voxel_filter_size())}; const auto voxel_filter_stop = std::chrono::steady_clock::now(); const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; @@ -274,19 +273,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const auto scan_matcher_start = std::chrono::steady_clock::now(); - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud high_resolution_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.high_resolution_adaptive_voxel_filter_options()); if (high_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty high resolution point cloud data."; return nullptr; } - sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( - options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = - low_resolution_adaptive_voxel_filter.Filter( - filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.low_resolution_adaptive_voxel_filter_options()); if (low_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty low resolution point cloud data."; return nullptr; diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index 4185b33600..d7c90288cc 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -17,7 +17,10 @@ #include "cartographer/sensor/internal/voxel_filter.h" #include +#include +#include +#include "absl/container/flat_hash_map.h" #include "cartographer/common/math.h" namespace cartographer { @@ -43,7 +46,7 @@ PointCloud AdaptivelyVoxelFiltered( // 'point_cloud' is already sparse enough. return point_cloud; } - PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud); + PointCloud result = VoxelFilter(point_cloud, options.max_length()); if (result.size() >= options.min_num_points()) { // Filtering with 'max_length' resulted in a sufficiently dense point cloud. return result; @@ -54,15 +57,14 @@ PointCloud AdaptivelyVoxelFiltered( for (float high_length = options.max_length(); high_length > 1e-2f * options.max_length(); high_length /= 2.f) { float low_length = high_length / 2.f; - result = VoxelFilter(low_length).Filter(point_cloud); + result = VoxelFilter(point_cloud, low_length); if (result.size() >= options.min_num_points()) { // Binary search to find the right amount of filtering. 'low_length' gave // a sufficiently dense 'result', 'high_length' did not. We stop when the // edge length is at most 10% off. while ((high_length - low_length) / low_length > 1e-1f) { const float mid_length = (low_length + high_length) / 2.f; - const PointCloud candidate = - VoxelFilter(mid_length).Filter(point_cloud); + const PointCloud candidate = VoxelFilter(point_cloud, mid_length); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; @@ -76,58 +78,77 @@ PointCloud AdaptivelyVoxelFiltered( return result; } -} // namespace +using VoxelKeyType = uint64_t; -PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { - PointCloud results; - for (const RangefinderPoint& point : point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); - } - } - return results; +VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point, + const float resolution) { + const Eigen::Array3f index = point.array() / resolution; + const uint64_t x = common::RoundToInt(index.x()); + const uint64_t y = common::RoundToInt(index.y()); + const uint64_t z = common::RoundToInt(index.z()); + return (x << 42) + (y << 21) + z; } -TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { - TimedPointCloud results; - for (const TimedRangefinderPoint& point : timed_point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); +template +std::vector RandomizedVoxelFilter(const std::vector& point_cloud, + const float resolution, + PointFunction&& point_function) { + // According to https://en.wikipedia.org/wiki/Reservoir_sampling + std::minstd_rand0 generator; + absl::flat_hash_map> + voxel_count_and_point_index; + for (size_t i = 0; i < point_cloud.size(); i++) { + auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex( + point_function(point_cloud[i]), resolution)]; + voxel.first++; + if (voxel.first == 1) { + voxel.second = i; + } else { + std::uniform_int_distribution<> distribution(1, voxel.first); + if (distribution(generator) == voxel.first) { + voxel.second = i; + } } } - return results; -} + std::vector points_used(point_cloud.size(), false); + for (const auto& voxel_and_index : voxel_count_and_point_index) { + points_used[voxel_and_index.second.second] = true; + } -std::vector VoxelFilter::Filter( - const std::vector& - range_measurements) { - std::vector results; - for (const auto& range_measurement : range_measurements) { - auto it_inserted = voxel_set_.insert( - IndexToKey(GetCellIndex(range_measurement.point_time.position))); - if (it_inserted.second) { - results.push_back(range_measurement); + std::vector results; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + results.push_back(point_cloud[i]); } } return results; } -VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) { - KeyType k_0(static_cast(index[0])); - KeyType k_1(static_cast(index[1])); - KeyType k_2(static_cast(index[2])); - return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2; +} // namespace + +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { + return RandomizedVoxelFilter( + point_cloud, resolution, + [](const RangefinderPoint& point) { return point.position; }); } -Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const { - Eigen::Array3f index = point.array() / resolution_; - return Eigen::Array3i(common::RoundToInt(index.x()), - common::RoundToInt(index.y()), - common::RoundToInt(index.z())); +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution) { + return RandomizedVoxelFilter( + timed_point_cloud, resolution, + [](const TimedRangefinderPoint& point) { return point.position; }); +} + +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution) { + return RandomizedVoxelFilter( + range_measurements, resolution, + [](const sensor::TimedPointCloudOriginData::RangeMeasurement& + range_measurement) { + return range_measurement.point_time.position; + }); } proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( @@ -140,13 +161,11 @@ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( return options; } -AdaptiveVoxelFilter::AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options) - : options_(options) {} - -PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options) { return AdaptivelyVoxelFiltered( - options_, FilterByMaxRange(point_cloud, options_.max_range())); + options, FilterByMaxRange(point_cloud, options.max_range())); } } // namespace sensor diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h index f14575b7c2..fd0c647968 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -19,7 +19,6 @@ #include -#include "absl/container/flat_hash_set.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" @@ -28,55 +27,20 @@ namespace cartographer { namespace sensor { -// Voxel filter for point clouds. For each voxel, the assembled point cloud -// contains the first point that fell into it from any of the inserted point -// clouds. -class VoxelFilter { - public: - // 'size' is the length of a voxel edge. - explicit VoxelFilter(float size) : resolution_(size) {} - - VoxelFilter(const VoxelFilter&) = delete; - VoxelFilter& operator=(const VoxelFilter&) = delete; - - // Returns a voxel filtered copy of 'point_cloud'. - PointCloud Filter(const PointCloud& point_cloud); - - // Same for TimedPointCloud. - TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); - - // Same for RangeMeasurement. - std::vector Filter( - const std::vector& - range_measurements); - - private: - using KeyType = std::bitset<3 * 32>; - - static KeyType IndexToKey(const Eigen::Array3i& index); - - Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const; - - float resolution_; - absl::flat_hash_set voxel_set_; -}; +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution); proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary); -class AdaptiveVoxelFilter { - public: - explicit AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options); - - AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; - AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; - - PointCloud Filter(const PointCloud& point_cloud) const; - - private: - const proto::AdaptiveVoxelFilterOptions options_; -}; +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options); } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index 320a99e3fc..4c18f1e5f2 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -24,33 +24,38 @@ namespace cartographer { namespace sensor { namespace { -using ::testing::ContainerEq; - -TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { - PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}, - {Eigen::Vector3f{0.1f, -0.1f, 0.1f}}, - {Eigen::Vector3f{0.3f, -0.1f, 0.f}}, - {Eigen::Vector3f{0.f, 0.f, 0.1f}}}; - EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); +using ::testing::Contains; + +TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { + const PointCloud point_cloud({{{0.f, 0.f, 0.f}}, + {{0.1f, -0.1f, 0.1f}}, + {{0.3f, -0.1f, 0.f}}, + {{0.f, 0.f, 0.1f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + ASSERT_EQ(result.size(), 2); + EXPECT_THAT(point_cloud, Contains(result[0])); + EXPECT_THAT(point_cloud, Contains(result[1])); + EXPECT_THAT(result, Contains(point_cloud[2])); } TEST(VoxelFilterTest, HandlesLargeCoordinates) { - PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}}, - {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}}, - {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}}, - {Eigen::Vector3f{-200000.f, 0.f, 0.f}}}; - EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[3]})); + const PointCloud point_cloud({{{100000.f, 0.f, 0.f}}, + {{100000.001f, -0.0001f, 0.0001f}}, + {{100000.003f, -0.0001f, 0.f}}, + {{-200000.f, 0.f, 0.f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.01f); + EXPECT_EQ(result.size(), 2); + EXPECT_THAT(result, Contains(point_cloud[3])); } TEST(VoxelFilterTest, IgnoresTime) { TimedPointCloud timed_point_cloud; for (int i = 0; i < 100; ++i) { - timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i}); + timed_point_cloud.push_back({{-100.f, 0.3f, 0.4f}, 1.f * i}); } - EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud), - ContainerEq(TimedPointCloud{timed_point_cloud[0]})); + const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f); + ASSERT_EQ(result.size(), 1); + EXPECT_THAT(timed_point_cloud, Contains(result[0])); } } // namespace From 9675d63926282f505f2e8333dd919429e49862c9 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 6 Oct 2020 15:41:04 +0200 Subject: [PATCH 60/99] Delete submap scan matchers that are unconstrained after trimming. (#1745) This improves the memory issue in pure localization mode as mentioned in issue #1737. Signed-off-by: Michael Grupp --- .../mapping/internal/2d/pose_graph_2d.cc | 24 +++++++++++++++++++ .../mapping/internal/3d/pose_graph_3d.cc | 24 +++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index fe85dce000..c533a0d355 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -1238,14 +1238,38 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->data_.constraints = std::move(constraints); } // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index d6410f7347..e40da19d74 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1218,14 +1218,38 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->data_.constraints = std::move(constraints); } // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data. From af00de3b3f38678ad183687439297c092ea61d60 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 6 Oct 2020 19:45:03 +0200 Subject: [PATCH 61/99] Don't add IMU data to 2D optimization problem. (#1754) It was constantly added regardless of motion filtering. Since it's not used it can be just ignored. The method still needs to be there to satisfy the interface. Signed-off-by: Michael Grupp --- .../mapping/internal/optimization/optimization_problem_2d.cc | 5 +++-- .../mapping/internal/optimization/optimization_problem_2d.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index b2ca862e8a..c61d7965e9 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -175,7 +175,8 @@ OptimizationProblem2D::~OptimizationProblem2D() {} void OptimizationProblem2D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - imu_data_.Append(trajectory_id, imu_data); + // IMU data is not used in 2D optimization, so we ignore this part of the + // interface. } void OptimizationProblem2D::AddOdometryData( @@ -207,7 +208,7 @@ void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, } void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { - imu_data_.Trim(node_data_, node_id); + empty_imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); fixed_frame_pose_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h index f8312317db..0a1e39dcd2 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -94,7 +94,7 @@ class OptimizationProblem2D return landmark_data_; } const sensor::MapByTime& imu_data() const override { - return imu_data_; + return empty_imu_data_; } const sensor::MapByTime& odometry_data() const override { @@ -128,7 +128,7 @@ class OptimizationProblem2D MapById node_data_; MapById submap_data_; std::map landmark_data_; - sensor::MapByTime imu_data_; + sensor::MapByTime empty_imu_data_; sensor::MapByTime odometry_data_; sensor::MapByTime fixed_frame_pose_data_; std::map trajectory_data_; From 1b31c017c4d262934609d6aeb1616c4f709a07a3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 7 Oct 2020 10:49:08 +0200 Subject: [PATCH 62/99] Use adaptive IMU weights in the PGO. (#1755) This weights IMU based on the time between nodes in the pose graph optimization. When moving slowly or stopping, IMU weights are reduced. This improves quality in these cases. The parameters are changed to approximately get the same behavior while moving as before for the examples. Signed-off-by: Wolfgang Hess --- .../optimization/optimization_problem_3d.cc | 14 ++++++++------ .../optimization/optimization_problem_3d_test.cc | 6 +++--- configuration_files/pose_graph.lua | 4 ++-- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 83d1777f4e..f5ca1a116d 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -395,14 +395,14 @@ void OptimizationProblem3D::Solve( const IntegrateImuResult result = IntegrateImu( imu_data, first_node_data.time, second_node_data.time, &imu_it); const auto next_node_it = std::next(node_it); + const common::Time first_time = first_node_data.time; + const common::Time second_time = second_node_data.time; + const common::Duration first_duration = second_time - first_time; if (next_node_it != trajectory_end && next_node_it->id.node_index == second_node_id.node_index + 1) { const NodeId third_node_id = next_node_it->id; const NodeSpec3D& third_node_data = next_node_it->data; - const common::Time first_time = first_node_data.time; - const common::Time second_time = second_node_data.time; const common::Time third_time = third_node_data.time; - const common::Duration first_duration = second_time - first_time; const common::Duration second_duration = third_time - second_time; const common::Time first_center = first_time + first_duration / 2; const common::Time second_center = second_time + second_duration / 2; @@ -421,8 +421,9 @@ void OptimizationProblem3D::Solve( result_center_to_center.delta_velocity; problem.AddResidualBlock( AccelerationCostFunction3D::CreateAutoDiffCostFunction( - options_.acceleration_weight(), delta_velocity, - common::ToSeconds(first_duration), + options_.acceleration_weight() / + common::ToSeconds(first_duration + second_duration), + delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration)), nullptr /* loss function */, C_nodes.at(second_node_id).rotation(), @@ -434,7 +435,8 @@ void OptimizationProblem3D::Solve( } problem.AddResidualBlock( RotationCostFunction3D::CreateAutoDiffCostFunction( - options_.rotation_weight(), result.delta_rotation), + options_.rotation_weight() / common::ToSeconds(first_duration), + result.delta_rotation), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), C_nodes.at(second_node_id).rotation(), trajectory_data.imu_calibration.data()); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 05bfac5d1c..92abbc6aca 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -39,8 +39,8 @@ class OptimizationProblem3DTest : public ::testing::Test { optimization::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { - acceleration_weight = 1e-4, - rotation_weight = 1e-2, + acceleration_weight = 2e-5, + rotation_weight = 1e-3, huber_scale = 1., local_slam_pose_translation_weight = 1e-2, local_slam_pose_rotation_weight = 1e-2, @@ -134,7 +134,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { Eigen::Vector3d::Zero()}); optimization_problem_.AddTrajectoryNode(kTrajectoryId, NodeSpec3D{now, pose, pose}); - now += common::FromSeconds(0.01); + now += common::FromSeconds(0.1); } std::vector constraints; diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index 572cd1a771..a962641695 100644 --- a/configuration_files/pose_graph.lua +++ b/configuration_files/pose_graph.lua @@ -63,8 +63,8 @@ POSE_GRAPH = { matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, - acceleration_weight = 1e3, - rotation_weight = 3e5, + acceleration_weight = 1.1e2, + rotation_weight = 1.6e4, local_slam_pose_translation_weight = 1e5, local_slam_pose_rotation_weight = 1e5, odometry_translation_weight = 1e5, From ca8a866996184d9943cb393e1d11bc382b0c9f15 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 7 Oct 2020 11:48:53 +0200 Subject: [PATCH 63/99] Constrain gravity to be positive. (#1756) This makes sure gravity never flips the sign and becomes negative. Signed-off-by: Wolfgang Hess --- .../mapping/internal/optimization/optimization_problem_3d.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index f5ca1a116d..4494045453 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -441,6 +441,9 @@ void OptimizationProblem3D::Solve( C_nodes.at(second_node_id).rotation(), trajectory_data.imu_calibration.data()); } + + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, 0.0); } } From 8ac967a50df6d2f46112d14473df57b30a5e177f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 9 Oct 2020 09:30:27 +0200 Subject: [PATCH 64/99] Add per-submap sampling. (#1758) This changes which submaps we select to attempt loop closing. The subsampling of candidates is changing from randomly sampling submap and node pairs to per-submap sampling. This enforces a better distribution of loop closure attempts across the submaps. This sampling achieves a much better performance which indicates that the approach used before was sub-optimal. Signed-off-by: Wolfgang Hess --- .../internal/constraints/constraint_builder_2d.cc | 11 +++++++++-- .../internal/constraints/constraint_builder_2d.h | 3 ++- .../internal/constraints/constraint_builder_3d.cc | 11 +++++++++-- .../internal/constraints/constraint_builder_3d.h | 3 ++- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index dcdb1d1120..ad3dfb7ec6 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" @@ -61,7 +63,6 @@ ConstraintBuilder2D::ConstraintBuilder2D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder2D::~ConstraintBuilder2D() { @@ -81,7 +82,12 @@ void ConstraintBuilder2D::MaybeAddConstraint( options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -305,6 +311,7 @@ void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index 272f7dcfce..6667fdb57b 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -160,8 +161,8 @@ class ConstraintBuilder2D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher2D ceres_scan_matcher_; // Histogram of scan matcher scores. diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index 878aaa032f..ca9ff71682 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" @@ -63,7 +65,6 @@ ConstraintBuilder3D::ConstraintBuilder3D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder3D::~ConstraintBuilder3D() { @@ -84,7 +85,12 @@ void ConstraintBuilder3D::MaybeAddConstraint( .norm() > options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -336,6 +342,7 @@ void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index fb0edc8b05..247a9da62c 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -169,8 +170,8 @@ class ConstraintBuilder3D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher3D ceres_scan_matcher_; // Histograms of scan matcher scores. From ee98a92845f7ded3c7e509d32f245f3860d23001 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 13 Oct 2020 15:57:45 +0200 Subject: [PATCH 65/99] Add new sensor::PointCloud interface. (#1759) These changes are necessary in order to later add intensities. Signed-off-by: Wolfgang Hess --- .../io/hybrid_grid_points_processor.cc | 4 +-- cartographer/io/points_batch.cc | 2 +- cartographer/io/points_batch.h | 4 +-- .../io/probability_grid_points_processor.cc | 5 +-- .../probability_grid_points_processor_test.cc | 5 +-- .../mapping/2d/tsdf_range_data_inserter_2d.cc | 6 ++-- .../mapping/3d/range_data_inserter_3d_test.cc | 14 ++++---- .../internal/2d/normal_estimation_2d_test.cc | 27 +++++++-------- .../occupied_space_cost_function_2d_test.cc | 4 +-- .../tsdf_match_cost_function_2d_test.cc | 8 ++--- .../fast_correlative_scan_matcher_3d_test.cc | 26 ++++++++++----- .../scan_matching/rotational_scan_matcher.cc | 2 +- cartographer/mapping/trajectory_node_test.cc | 21 +++++++----- .../sensor/compressed_point_cloud_test.cc | 17 +++++----- cartographer/sensor/internal/voxel_filter.cc | 9 +++-- cartographer/sensor/internal/voxel_filter.h | 2 ++ .../sensor/internal/voxel_filter_test.cc | 8 ++--- cartographer/sensor/point_cloud.cc | 31 ++++++++++++++--- cartographer/sensor/point_cloud.h | 33 +++++++++++++++++-- cartographer/sensor/point_cloud_test.cc | 4 +-- cartographer/sensor/range_data.cc | 7 ++-- 21 files changed, 155 insertions(+), 84 deletions(-) diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index 32525fb473..a573498096 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -39,8 +39,8 @@ HybridGridPointsProcessor::FromDictionary( } void HybridGridPointsProcessor::Process(std::unique_ptr batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &hybrid_grid_); + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_); next_->Process(std::move(batch)); } diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc index ea0fe4ca16..cb4848c029 100644 --- a/cartographer/io/points_batch.cc +++ b/cartographer/io/points_batch.cc @@ -21,7 +21,7 @@ namespace io { void RemovePoints(absl::flat_hash_set to_remove, PointsBatch* batch) { const int new_num_points = batch->points.size() - to_remove.size(); - sensor::PointCloud points; + std::vector points; points.reserve(new_num_points); std::vector intensities; if (!batch->intensities.empty()) { diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index 752966ecfd..cc1d6bc42e 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -25,7 +25,7 @@ #include "absl/container/flat_hash_set.h" #include "cartographer/common/time.h" #include "cartographer/io/color.h" -#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/rangefinder_point.h" namespace cartographer { namespace io { @@ -53,7 +53,7 @@ struct PointsBatch { int trajectory_id; // Geometry of the points in the map frame. - sensor::PointCloud points; + std::vector points; // Intensities are optional and may be unspecified. The meaning of these // intensity values varies by device. For example, the VLP16 provides values diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 1d746f9574..907b995f29 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -126,8 +126,9 @@ ProbabilityGridPointsProcessor::FromDictionary( void ProbabilityGridPointsProcessor::Process( std::unique_ptr batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &probability_grid_); + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, + &probability_grid_); next_->Process(std::move(batch)); } diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index 99753bb5be..8c48f6d697 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -85,8 +85,9 @@ std::vector CreateExpectedProbabilityGrid( mapping::ValueConversionTables conversion_tables; auto probability_grid = CreateProbabilityGrid( probability_grid_options->GetDouble("resolution"), &conversion_tables); - range_data_inserter.Insert({points_batch->origin, points_batch->points, {}}, - &probability_grid); + range_data_inserter.Insert( + {points_batch->origin, sensor::PointCloud(points_batch->points), {}}, + &probability_grid); std::vector probability_grid_proto( probability_grid.ToProto().ByteSize()); diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc index d7619d089e..7a6a4e7cc6 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc @@ -142,9 +142,11 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, std::vector normals; if (options_.project_sdf_distance_to_scan_normal() || scale_update_weight_angle_scan_normal_to_ray) { - std::sort(sorted_range_data.returns.begin(), - sorted_range_data.returns.end(), + std::vector returns = + sorted_range_data.returns.points(); + std::sort(returns.begin(), returns.end(), RangeDataSorter(sorted_range_data.origin)); + sorted_range_data.returns = sensor::PointCloud(std::move(returns)); normals = EstimateNormals(sorted_range_data, options_.normal_estimation_options()); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index eb051f1899..0abc75ad38 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -41,12 +41,14 @@ class RangeDataInserter3DTest : public ::testing::Test { void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); - sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}}, - {Eigen::Vector3f{-2.f, 0.f, 4.f}}, - {Eigen::Vector3f{-1.f, 1.f, 4.f}}, - {Eigen::Vector3f{0.f, 2.f, 4.f}}}; - range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, - &hybrid_grid_); + std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns), {}}, + &hybrid_grid_); } float GetProbability(float x, float y, float z) const { diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc index e32dcb49e8..bb6c453d4d 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -45,9 +45,9 @@ TEST(NormalEstimation2DTest, SinglePoint) { const double angle = static_cast(angle_idx) / static_cast(num_angles) * 2. * M_PI - M_PI; - range_data.returns.clear(); - range_data.returns.push_back( - {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); + range_data.returns = sensor::PointCloud( + {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.} + .cast()}}); std::vector normals; normals = EstimateNormals(range_data, options); EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), @@ -75,30 +75,27 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { EXPECT_NEAR(normal, -M_PI_2, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}}, + {Eigen::Vector3f{1.f, 0.f, 0.f}}, + {Eigen::Vector3f{1.f, -1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(std::abs(normal), M_PI, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}}, + {Eigen::Vector3f{0.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, -1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, M_PI_2, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, 0.f, 0.f}}, + {Eigen::Vector3f{-1.f, 1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, 0, 1e-4); diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc index 4cc82e266d..c9ef1828a5 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -33,7 +33,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { ValueConversionTables conversion_tables; ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), &conversion_tables); - sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; + sensor::PointCloud point_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); ceres::Problem problem; std::unique_ptr cost_function( CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); @@ -54,4 +54,4 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { } // namespace } // namespace scan_matching } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index 5c6b5841c2..3311ace09f 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -72,7 +72,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test { }; TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { - const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -88,7 +88,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { InsertPointcloud(); - const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -108,7 +108,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.1, 0.}}; @@ -139,7 +139,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.4, 0.}}; diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 100a72fd83..85ea48ae24 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -40,13 +40,18 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {} void SetUp() override { - point_cloud_ = { - {Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)}, - {Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)}, - {Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)}, - {Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)}, - {Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)}, - {Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}}; + point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)}, + {Eigen::Vector3f(4.5f, 0.f, 0.f)}, + {Eigen::Vector3f(5.f, 0.f, 0.f)}, + {Eigen::Vector3f(5.5f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.5f, 0.f)}, + {Eigen::Vector3f(0.f, 5.f, 0.f)}, + {Eigen::Vector3f(0.f, 5.5f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.5f)}, + {Eigen::Vector3f(0.f, 0.f, 5.f)}, + {Eigen::Vector3f(0.f, 0.f, 5.5f)}}); } transform::Rigid3f GetRandomPose() { @@ -159,7 +164,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), - CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; @@ -188,7 +194,9 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), - CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore); + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), + kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; } diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 0e4b5e0202..a584ff45cf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -68,7 +68,7 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice, // will add the angle between points to the histogram with the maximum weight. // This is to reject, e.g., the angles observed on the ceiling and floor. const Eigen::Vector3f centroid = ComputeCentroid(slice); - Eigen::Vector3f last_point_position = slice.front().position; + Eigen::Vector3f last_point_position = slice.points().front().position; for (const sensor::RangefinderPoint& point : slice) { const Eigen::Vector2f delta = (point.position - last_point_position).head<2>(); diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc index 029681c6b9..c927a8d776 100644 --- a/cartographer/mapping/trajectory_node_test.cc +++ b/cartographer/mapping/trajectory_node_test.cc @@ -33,11 +33,14 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { common::FromUniversal(42), Eigen::Quaterniond(1., 2., -3., -4.), sensor::CompressedPointCloud( - {{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}}) + sensor::PointCloud({{Eigen::Vector3f{1.f, 2.f, 0.f}}, + {Eigen::Vector3f{0.f, 0.f, 1.f}}})) .Decompress(), - sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}) + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})) .Decompress(), - sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}) + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})) .Decompress(), Eigen::VectorXf::Unit(20, 4), transform::Rigid3d({1., 2., 3.}, @@ -46,12 +49,12 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { const TrajectoryNode::Data actual = FromProto(proto); EXPECT_EQ(expected.time, actual.time); EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment)); - EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud, - actual.filtered_gravity_aligned_point_cloud); - EXPECT_EQ(expected.high_resolution_point_cloud, - actual.high_resolution_point_cloud); - EXPECT_EQ(expected.low_resolution_point_cloud, - actual.low_resolution_point_cloud); + EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud.points(), + actual.filtered_gravity_aligned_point_cloud.points()); + EXPECT_EQ(expected.high_resolution_point_cloud.points(), + actual.high_resolution_point_cloud.points()); + EXPECT_EQ(expected.low_resolution_point_cloud.points(), + actual.low_resolution_point_cloud.points()); EXPECT_EQ(expected.rotational_scan_matcher_histogram, actual.rotational_scan_matcher_histogram); EXPECT_THAT(actual.local_pose, diff --git a/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/sensor/compressed_point_cloud_test.cc index 341ad7841d..7dcd6bc131 100644 --- a/cartographer/sensor/compressed_point_cloud_test.cc +++ b/cartographer/sensor/compressed_point_cloud_test.cc @@ -48,10 +48,10 @@ MATCHER_P(ApproximatelyEquals, expected, // Helper function to test the mapping of a single point. Includes test for // recompressing the same point again. void TestPoint(const Eigen::Vector3f& p) { - CompressedPointCloud compressed({{p}}); + CompressedPointCloud compressed(PointCloud({{p}})); EXPECT_EQ(1, compressed.size()); EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); - CompressedPointCloud recompressed({*compressed.begin()}); + CompressedPointCloud recompressed(PointCloud({*compressed.begin()})); EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p)); } @@ -70,18 +70,19 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) { } TEST(CompressPointCloudTest, Compresses) { - const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)}, - {Eigen::Vector3f(0.839f, 0, 0)}, - {Eigen::Vector3f(0.840f, 0, 0)}}); + const CompressedPointCloud compressed( + PointCloud({{Eigen::Vector3f(0.838f, 0, 0)}, + {Eigen::Vector3f(0.839f, 0, 0)}, + {Eigen::Vector3f(0.840f, 0, 0)}})); EXPECT_FALSE(compressed.empty()); EXPECT_EQ(3, compressed.size()); const PointCloud decompressed = compressed.Decompress(); EXPECT_EQ(3, decompressed.size()); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0)))); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0)))); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0)))); } diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index d7c90288cc..67f2814464 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -126,12 +126,17 @@ std::vector RandomizedVoxelFilter(const std::vector& point_cloud, } // namespace -PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { +std::vector VoxelFilter( + const std::vector& points, const float resolution) { return RandomizedVoxelFilter( - point_cloud, resolution, + points, resolution, [](const RangefinderPoint& point) { return point.position; }); } +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { + return PointCloud(VoxelFilter(point_cloud.points(), resolution)); +} + TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, const float resolution) { return RandomizedVoxelFilter( diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h index fd0c647968..a99f861650 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -27,6 +27,8 @@ namespace cartographer { namespace sensor { +std::vector VoxelFilter( + const std::vector& points, const float resolution); PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, const float resolution); diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index 4c18f1e5f2..db6cd3f1e8 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -33,9 +33,9 @@ TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { {{0.f, 0.f, 0.1f}}}); const PointCloud result = VoxelFilter(point_cloud, 0.3f); ASSERT_EQ(result.size(), 2); - EXPECT_THAT(point_cloud, Contains(result[0])); - EXPECT_THAT(point_cloud, Contains(result[1])); - EXPECT_THAT(result, Contains(point_cloud[2])); + EXPECT_THAT(point_cloud.points(), Contains(result[0])); + EXPECT_THAT(point_cloud.points(), Contains(result[1])); + EXPECT_THAT(result.points(), Contains(point_cloud[2])); } TEST(VoxelFilterTest, HandlesLargeCoordinates) { @@ -45,7 +45,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) { {{-200000.f, 0.f, 0.f}}}); const PointCloud result = VoxelFilter(point_cloud, 0.01f); EXPECT_EQ(result.size(), 2); - EXPECT_THAT(result, Contains(point_cloud[3])); + EXPECT_THAT(result.points(), Contains(point_cloud[3])); } TEST(VoxelFilterTest, IgnoresTime) { diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 959017c424..01a1416a6a 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -22,14 +22,35 @@ namespace cartographer { namespace sensor { +PointCloud::PointCloud() {} +PointCloud::PointCloud(std::vector points) + : points_(std::move(points)) {} + +size_t PointCloud::size() const { return points_.size(); } +bool PointCloud::empty() const { return points_.empty(); } + +const std::vector& PointCloud::points() const { + return points_; +} +const PointCloud::PointType& PointCloud::operator[](const size_t index) const { + return points_[index]; +} + +PointCloud::ConstIterator PointCloud::begin() const { return points_.begin(); } +PointCloud::ConstIterator PointCloud::end() const { return points_.end(); } + +void PointCloud::push_back(PointCloud::PointType value) { + points_.push_back(std::move(value)); +} + PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform) { - PointCloud result; - result.reserve(point_cloud.size()); - for (const RangefinderPoint& point : point_cloud) { - result.emplace_back(transform * point); + std::vector points; + points.reserve(point_cloud.size()); + for (const RangefinderPoint& point : point_cloud.points()) { + points.emplace_back(transform * point); } - return result; + return PointCloud(points); } TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index d2c338d393..f20307fba1 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -28,9 +28,34 @@ namespace cartographer { namespace sensor { -// Stores 3D positions of points. -// For 2D points, the third entry is 0.f. -using PointCloud = std::vector; +// Stores 3D positions of points together with some additional data, e.g. +// intensities. +struct PointCloud { + public: + using PointType = RangefinderPoint; + + PointCloud(); + explicit PointCloud(std::vector points); + + // Returns the number of points in the point cloud. + size_t size() const; + // Checks whether there are any points in the point cloud. + bool empty() const; + + const std::vector& points() const; + const PointType& operator[](const size_t index) const; + + // Iterator over the points in the point cloud. + using ConstIterator = std::vector::const_iterator; + ConstIterator begin() const; + ConstIterator end() const; + + void push_back(PointType value); + + private: + // For 2D points, the third entry is 0.f. + std::vector points_; +}; // Stores 3D positions of points with their relative measurement time in the // fourth entry. Time is in seconds, increasing and relative to the moment when @@ -39,6 +64,8 @@ using PointCloud = std::vector; // third entry is 0.f (and the fourth entry is time). using TimedPointCloud = std::vector; +// TODO(wohe): Retained for cartographer_ros. To be removed once it is no +// longer used there. struct PointCloudWithIntensities { TimedPointCloud points; std::vector intensities; diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 4f547feb43..39cc05f724 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -27,8 +27,8 @@ namespace { TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; - point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}}); - point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}}); + point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}}); + point_cloud.push_back({{Eigen::Vector3f{3.5f, 0.5f, 42.f}}}); const PointCloud transformed_point_cloud = TransformPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 1151545634..c30e68d36b 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -53,7 +53,7 @@ proto::RangeData ToProto(const RangeData& range_data) { } RangeData FromProto(const proto::RangeData& proto) { - PointCloud returns; + std::vector returns; if (proto.returns_size() > 0) { returns.reserve(proto.returns().size()); for (const auto& point_proto : proto.returns()) { @@ -65,7 +65,7 @@ RangeData FromProto(const proto::RangeData& proto) { returns.push_back({transform::ToEigen(point_proto)}); } } - PointCloud misses; + std::vector misses; if (proto.misses_size() > 0) { misses.reserve(proto.misses().size()); for (const auto& point_proto : proto.misses()) { @@ -77,7 +77,8 @@ RangeData FromProto(const proto::RangeData& proto) { misses.push_back({transform::ToEigen(point_proto)}); } } - return RangeData{transform::ToEigen(proto.origin()), returns, misses}; + return RangeData{transform::ToEigen(proto.origin()), PointCloud(returns), + PointCloud(misses)}; } } // namespace sensor From cad33789290b0d157f2ea991b42071b07b03fbab Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 14 Oct 2020 11:36:47 +0200 Subject: [PATCH 66/99] Add a cost function to take intensities into account. (#1760) Adds (optional) intensities to sensor::PointCloud and uses it in a cost function. Not yet in use. Signed-off-by: Wolfgang Hess --- .../intensity_cost_function_3d.cc | 25 +++++ .../intensity_cost_function_3d.h | 98 +++++++++++++++++++ .../intensity_cost_function_3d_test.cc | 66 +++++++++++++ cartographer/sensor/point_cloud.cc | 10 ++ cartographer/sensor/point_cloud.h | 7 +- 5 files changed, 205 insertions(+), 1 deletion(-) create mode 100644 cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc create mode 100644 cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h create mode 100644 cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc new file mode 100644 index 0000000000..fe16ff5dac --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc @@ -0,0 +1,25 @@ +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// This method is defined here instead of the header file as it was observed +// that defining it in the header file has a negative impact on the runtime +// performance. +ceres::CostFunction* IntensityCostFunction3D::CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) { + CHECK(!point_cloud.intensities().empty()); + return new ceres::AutoDiffCostFunction< + IntensityCostFunction3D, ceres::DYNAMIC /* residuals */, + 3 /* translation variables */, 4 /* rotation variables */>( + new IntensityCostFunction3D(scaling_factor, intensity_threshold, + point_cloud, hybrid_grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h new file mode 100644 index 0000000000..59f1c29694 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h @@ -0,0 +1,98 @@ +/* + * Copyright 2019 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_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into space +// for which different intensity has been observed, i.e. at voxels with different +// values. Only points up to a certain threshold are evaluated which is intended +// to ignore data from retroreflections. +class IntensityCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid); + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + private: + IntensityCostFunction3D(const double scaling_factor, + const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + intensity_threshold_(intensity_threshold), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + IntensityCostFunction3D(const IntensityCostFunction3D&) = delete; + IntensityCostFunction3D& operator=(const IntensityCostFunction3D&) = delete; + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + if (point_cloud_.intensities()[i] > intensity_threshold_) { + residual[i] = T(0.f); + } else { + const Eigen::Matrix point = point_cloud_[i].position.cast(); + const T intensity = T(point_cloud_.intensities()[i]); + + const Eigen::Matrix world = transform * point; + const T interpolated_intensity = + interpolated_grid_.GetInterpolatedValue(world[0], world[1], + world[2]); + residual[i] = scaling_factor_ * (interpolated_intensity - intensity); + } + } + return true; + } + + const double scaling_factor_; + // We will ignore returns with intensity above this threshold. + const float intensity_threshold_; + const sensor::PointCloud& point_cloud_; + const InterpolatedIntensityGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc new file mode 100644 index 0000000000..90fc8c4b65 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2019 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/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +#include +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleNear; +using ::testing::ElementsAre; + +TEST(IntensityCostFunction3DTest, SmokeTest) { + const sensor::PointCloud point_cloud( + {{{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}, + {50.f, 100.f, 150.f}); + IntensityHybridGrid hybrid_grid(0.3f); + hybrid_grid.AddIntensity(hybrid_grid.GetCellIndex({0.f, 0.f, 0.f}), 50.f); + + std::unique_ptr cost_function( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + /*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud, + hybrid_grid)); + + const std::array translation{{0., 0., 0.}}; + const std::array rotation{{1., 0., 0., 0.}}; + + const std::array parameter_blocks{ + {translation.data(), rotation.data()}}; + std::array residuals; + + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + /*jacobians=*/nullptr); + EXPECT_THAT(residuals, + ElementsAre(DoubleNear(0., 1e-9), DoubleNear(-100., 1e-9), + DoubleNear(0., 1e-9))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 01a1416a6a..5845f232d7 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -25,6 +25,13 @@ namespace sensor { PointCloud::PointCloud() {} PointCloud::PointCloud(std::vector points) : points_(std::move(points)) {} +PointCloud::PointCloud(std::vector points, + std::vector intensities) + : points_(std::move(points)), intensities_(std::move(intensities)) { + if (!intensities_.empty()) { + CHECK_EQ(points_.size(), intensities_.size()); + } +} size_t PointCloud::size() const { return points_.size(); } bool PointCloud::empty() const { return points_.empty(); } @@ -32,6 +39,9 @@ bool PointCloud::empty() const { return points_.empty(); } const std::vector& PointCloud::points() const { return points_; } +const std::vector& PointCloud::intensities() const { + return intensities_; +} const PointCloud::PointType& PointCloud::operator[](const size_t index) const { return points_[index]; } diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index f20307fba1..19a40aa52f 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -30,12 +30,13 @@ namespace sensor { // Stores 3D positions of points together with some additional data, e.g. // intensities. -struct PointCloud { +class PointCloud { public: using PointType = RangefinderPoint; PointCloud(); explicit PointCloud(std::vector points); + PointCloud(std::vector points, std::vector intensities); // Returns the number of points in the point cloud. size_t size() const; @@ -43,6 +44,7 @@ struct PointCloud { bool empty() const; const std::vector& points() const; + const std::vector& intensities() const; const PointType& operator[](const size_t index) const; // Iterator over the points in the point cloud. @@ -55,6 +57,9 @@ struct PointCloud { private: // For 2D points, the third entry is 0.f. std::vector points_; + // Intensities are optional. If non-empty, they must have the same size as + // points. + std::vector intensities_; }; // Stores 3D positions of points with their relative measurement time in the From 81d34ef18548bb35d0e1c1c4832d3f3e6031603b Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 14 Oct 2020 18:04:12 +0200 Subject: [PATCH 67/99] Add intensity cost function to the Ceres scan matcher. (#1761) Not yet used. Signed-off-by: Wolfgang Hess --- .../3d/local_trajectory_builder_3d.cc | 6 ++- .../3d/scan_matching/ceres_scan_matcher_3d.cc | 46 +++++++++++++++++-- .../3d/scan_matching/ceres_scan_matcher_3d.h | 11 +++-- .../ceres_scan_matcher_3d_test.cc | 28 +++++++++-- .../constraints/constraint_builder_3d.cc | 6 ++- .../ceres_scan_matcher_options_3d.proto | 14 +++++- 6 files changed, 93 insertions(+), 18 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 361f724cf0..a9bf65f3ae 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -86,9 +86,11 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( (matching_submap->local_pose().inverse() * pose_prediction).translation(), initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, - &matching_submap->high_resolution_hybrid_grid()}, + &matching_submap->high_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}, {&low_resolution_point_cloud_in_tracking, - &matching_submap->low_resolution_hybrid_grid()}}, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, &pose_observation_in_submap, &summary); kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance = (pose_observation_in_submap.translation() - diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc index 7eb8fb5629..2c107532ba 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -23,6 +23,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" @@ -48,6 +49,24 @@ proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( options.add_occupied_space_weight( parameter_dictionary->GetDouble(lua_identifier)); } + for (int i = 0;; ++i) { + const std::string lua_identifier = + "intensity_cost_function_options_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + const auto intensity_cost_function_options_dictionary = + parameter_dictionary->GetDictionary(lua_identifier); + auto* intensity_cost_function_options = + options.add_intensity_cost_function_options(); + intensity_cost_function_options->set_weight( + intensity_cost_function_options_dictionary->GetDouble("weight")); + intensity_cost_function_options->set_huber_scale( + intensity_cost_function_options_dictionary->GetDouble("huber_scale")); + intensity_cost_function_options->set_intensity_threshold( + intensity_cost_function_options_dictionary->GetDouble( + "intensity_threshold")); + } options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( @@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D( void CeresScanMatcher3D::Match( const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* const pose_estimate, - ceres::Solver::Summary* const summary) { + ceres::Solver::Summary* const summary) const { ceres::Problem problem; optimization::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, @@ -91,8 +110,9 @@ void CeresScanMatcher3D::Match( for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = - *point_clouds_and_hybrid_grids[i].first; - const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; + *point_clouds_and_hybrid_grids[i].point_cloud; + const HybridGrid& hybrid_grid = + *point_clouds_and_hybrid_grids[i].hybrid_grid; problem.AddResidualBlock( OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( options_.occupied_space_weight(i) / @@ -100,7 +120,25 @@ void CeresScanMatcher3D::Match( point_cloud, hybrid_grid), nullptr /* loss function */, ceres_pose.translation(), ceres_pose.rotation()); + if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) { + CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.); + CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.); + CHECK_GT( + options_.intensity_cost_function_options(i).intensity_threshold(), 0); + const IntensityHybridGrid& intensity_hybrid_grid = + *point_clouds_and_hybrid_grids[i].intensity_hybrid_grid; + problem.AddResidualBlock( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + options_.intensity_cost_function_options(i).weight() / + std::sqrt(static_cast(point_cloud.size())), + options_.intensity_cost_function_options(i).intensity_threshold(), + point_cloud, intensity_hybrid_grid), + new ceres::HuberLoss( + options_.intensity_cost_function_options(i).huber_scale()), + ceres_pose.translation(), ceres_pose.rotation()); + } } + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h index ffbaa0cdcc..17c6066b4b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h @@ -34,8 +34,11 @@ namespace scan_matching { proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); -using PointCloudAndHybridGridPointers = - std::pair; +struct PointCloudAndHybridGridsPointers { + const sensor::PointCloud* point_cloud; + const HybridGrid* hybrid_grid; + const IntensityHybridGrid* intensity_hybrid_grid; // optional +}; // This scan matcher uses Ceres to align scans with an existing map. class CeresScanMatcher3D { @@ -50,10 +53,10 @@ class CeresScanMatcher3D { // 'summary'. void Match(const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* pose_estimate, - ceres::Solver::Summary* summary); + ceres::Solver::Summary* summary) const; private: const proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc index 52d40258ab..68a67703e0 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test { protected: CeresScanMatcher3DTest() : hybrid_grid_(1.f), + intensity_hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { + std::vector points; + std::vector intensities; for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { - point_cloud_.push_back({point}); + points.push_back({point}); + intensities.push_back(50); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + intensity_hybrid_grid_.AddIntensity( + intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast() * + point), + 50); } + point_cloud_ = sensor::PointCloud(points, intensities); auto parameter_dictionary = common::MakeDictionary(R"text( return { occupied_space_weight_0 = 1., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 55, + intensity_threshold = 100, + }, translation_weight = 0.01, rotation_weight = 0.1, only_optimize_yaw = false, @@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test { transform::Rigid3d pose; ceres::Solver::Summary summary; - ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, - {{&point_cloud_, &hybrid_grid_}}, &pose, - &summary); + + IntensityHybridGrid* intensity_hybrid_grid_ptr = + point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_; + + ceres_scan_matcher_->Match( + initial_pose.translation(), initial_pose, + {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose, + &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index ca9ff71682..87b3742b53 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -267,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint( ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), match_result->pose_estimate, {{&constant_data->high_resolution_point_cloud, - submap_scan_matcher.high_resolution_hybrid_grid}, + submap_scan_matcher.high_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}, {&constant_data->low_resolution_point_cloud, - submap_scan_matcher.low_resolution_hybrid_grid}}, + submap_scan_matcher.low_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}}, &constraint_transform, &unused_summary); constraint->reset(new Constraint{ diff --git a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto index 8c9030a83a..0f80184159 100644 --- a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto +++ b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto @@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 7 +message IntensityCostFunctionOptions { + double weight = 1; + double huber_scale = 2; + // Ignore ranges with intensity above this threshold. + float intensity_threshold = 3; +} + +// NEXT ID: 8 message CeresScanMatcherOptions3D { - // Scaling parameters for each cost functor. + // Scaling parameters for each occupied space cost functor. repeated double occupied_space_weight = 1; double translation_weight = 2; double rotation_weight = 3; @@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D { // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ common.proto.CeresSolverOptions ceres_solver_options = 6; + + // Scaling parameters for each intensity cost functor. + repeated IntensityCostFunctionOptions intensity_cost_function_options = 7; } From 952f59d4999bf242dc58d48caf039bd5d2b0cb94 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 16 Oct 2020 10:52:28 +0200 Subject: [PATCH 68/99] Take intensities into account in PointCloud transforms and filters. (#1763) Adds a new method to sensor::PointCloud, copy_if, which copies all points satisfying a provided condition, together with the associated intensities (if they exist). Adapts transforms/filters to use this new method. Signed-off-by: Wolfgang Hess --- cartographer/mapping/3d/submap_3d.cc | 12 +++---- cartographer/sensor/internal/voxel_filter.cc | 10 ++---- cartographer/sensor/point_cloud.cc | 12 +++---- cartographer/sensor/point_cloud.h | 29 +++++++++++++++ cartographer/sensor/point_cloud_test.cc | 37 ++++++++++++++++++++ 5 files changed, 78 insertions(+), 22 deletions(-) diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 97c5d0fac8..7d75bd94e2 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -37,16 +37,14 @@ struct PixelData { }; // Filters 'range_data', retaining only the returns that have no more than -// 'max_range' distance from the origin. Removes misses and reflectivity -// information. +// 'max_range' distance from the origin. Removes misses. sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, const float max_range) { sensor::RangeData result{range_data.origin, {}, {}}; - for (const sensor::RangefinderPoint& hit : range_data.returns) { - if ((hit.position - range_data.origin).norm() <= max_range) { - result.returns.push_back(hit); - } - } + result.returns = + range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) { + return (point.position - range_data.origin).norm() <= max_range; + }); return result; } diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index 67f2814464..b1640ad7a2 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -30,13 +30,9 @@ namespace { PointCloud FilterByMaxRange(const PointCloud& point_cloud, const float max_range) { - PointCloud result; - for (const RangefinderPoint& point : point_cloud) { - if (point.position.norm() <= max_range) { - result.push_back(point); - } - } - return result; + return point_cloud.copy_if([max_range](const RangefinderPoint& point) { + return point.position.norm() <= max_range; + }); } PointCloud AdaptivelyVoxelFiltered( diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 5845f232d7..f616c190ca 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -60,7 +60,7 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud, for (const RangefinderPoint& point : point_cloud.points()) { points.emplace_back(transform * point); } - return PointCloud(points); + return PointCloud(points, point_cloud.intensities()); } TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, @@ -75,13 +75,9 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, const float max_z) { - PointCloud cropped_point_cloud; - for (const RangefinderPoint& point : point_cloud) { - if (min_z <= point.position.z() && point.position.z() <= max_z) { - cropped_point_cloud.push_back(point); - } - } - return cropped_point_cloud; + return point_cloud.copy_if([min_z, max_z](const RangefinderPoint& point) { + return min_z <= point.position.z() && point.position.z() <= max_z; + }); } } // namespace sensor diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index 19a40aa52f..289e367b6d 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -54,6 +54,35 @@ class PointCloud { void push_back(PointType value); + // Creates a PointCloud consisting of all the points for which `predicate` + // returns true, together with the corresponding intensities. + template + PointCloud copy_if(UnaryPredicate predicate) const { + std::vector points; + std::vector intensities; + + // Note: benchmarks show that it is better to have this conditional outside + // the loop. + if (intensities_.empty()) { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + if (predicate(point)) { + points.push_back(point); + } + } + } else { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + if (predicate(point)) { + points.push_back(point); + intensities.push_back(intensities_[index]); + } + } + } + + return PointCloud(points, intensities); + } + private: // For 2D points, the third entry is 0.f. std::vector points_; diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 39cc05f724..47c6febcdd 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -19,12 +19,17 @@ #include #include "cartographer/transform/transform.h" +#include "gmock/gmock.h" #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { +using ::testing::ElementsAre; +using ::testing::FloatNear; +using ::testing::IsEmpty; + TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}}); @@ -49,6 +54,38 @@ TEST(PointCloudTest, TransformTimedPointCloud) { EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); } +TEST(PointCloudTest, CopyIf) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + + const PointCloud point_cloud(points); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_THAT(copied_point_cloud.intensities(), IsEmpty()); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); +} + +TEST(PointCloudTest, CopyIfWithIntensities) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + std::vector intensities = {0.f, 1.f, 2.f}; + + const PointCloud point_cloud(points, intensities); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_EQ(copied_point_cloud.intensities().size(), 2); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); + EXPECT_THAT(copied_point_cloud.intensities(), + ElementsAre(FloatNear(1.f, 1e-6), FloatNear(2.f, 1e-6))); +} + } // namespace } // namespace sensor } // namespace cartographer From d3473fca4a99f239980cfab58a88eb35fa59d2c0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 19 Oct 2020 12:14:50 +0200 Subject: [PATCH 69/99] Keep intensities in the voxel filter. (#1764) Signed-off-by: Wolfgang Hess --- cartographer/sensor/internal/voxel_filter.cc | 35 ++++++++++++++++--- .../sensor/internal/voxel_filter_test.cc | 23 ++++++++++++ 2 files changed, 54 insertions(+), 4 deletions(-) diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index b1640ad7a2..a6e5c0b574 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -86,9 +86,9 @@ VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point, } template -std::vector RandomizedVoxelFilter(const std::vector& point_cloud, - const float resolution, - PointFunction&& point_function) { +std::vector RandomizedVoxelFilterIndices( + const std::vector& point_cloud, const float resolution, + PointFunction&& point_function) { // According to https://en.wikipedia.org/wiki/Reservoir_sampling std::minstd_rand0 generator; absl::flat_hash_map> @@ -110,6 +110,15 @@ std::vector RandomizedVoxelFilter(const std::vector& point_cloud, for (const auto& voxel_and_index : voxel_count_and_point_index) { points_used[voxel_and_index.second.second] = true; } + return points_used; +} + +template +std::vector RandomizedVoxelFilter(const std::vector& point_cloud, + const float resolution, + PointFunction&& point_function) { + const std::vector points_used = + RandomizedVoxelFilterIndices(point_cloud, resolution, point_function); std::vector results; for (size_t i = 0; i < point_cloud.size(); i++) { @@ -130,7 +139,25 @@ std::vector VoxelFilter( } PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { - return PointCloud(VoxelFilter(point_cloud.points(), resolution)); + const std::vector points_used = RandomizedVoxelFilterIndices( + point_cloud.points(), resolution, + [](const RangefinderPoint& point) { return point.position; }); + + std::vector filtered_points; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + filtered_points.push_back(point_cloud[i]); + } + } + std::vector filtered_intensities; + CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size()); + for (size_t i = 0; i < point_cloud.intensities().size(); i++) { + if (points_used[i]) { + filtered_intensities.push_back(point_cloud.intensities()[i]); + } + } + return PointCloud(std::move(filtered_points), + std::move(filtered_intensities)); } TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index db6cd3f1e8..b84152f166 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -25,6 +25,7 @@ namespace sensor { namespace { using ::testing::Contains; +using ::testing::IsEmpty; TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { const PointCloud point_cloud({{{0.f, 0.f, 0.f}}, @@ -33,11 +34,32 @@ TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { {{0.f, 0.f, 0.1f}}}); const PointCloud result = VoxelFilter(point_cloud, 0.3f); ASSERT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); EXPECT_THAT(point_cloud.points(), Contains(result[0])); EXPECT_THAT(point_cloud.points(), Contains(result[1])); EXPECT_THAT(result.points(), Contains(point_cloud[2])); } +TEST(VoxelFilterTest, CorrectIntensities) { + std::vector points; + std::vector intensities; + for (int i = 0; i < 100; ++i) { + const float value = 0.1f * i; + // We add points with intensity equal to the z coordinate, so we can later + // verify that the resulting intensities are corresponding to the filtered + // points. + points.push_back({{-100.f, 0.3f, value}}); + intensities.push_back(value); + } + const PointCloud point_cloud(points, intensities); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + + ASSERT_EQ(result.intensities().size(), result.points().size()); + for (size_t i = 0; i < result.size(); ++i) { + ASSERT_NEAR(result[i].position.z(), result.intensities()[i], 1e-6); + } +} + TEST(VoxelFilterTest, HandlesLargeCoordinates) { const PointCloud point_cloud({{{100000.f, 0.f, 0.f}}, {{100000.001f, -0.0001f, 0.0001f}}, @@ -45,6 +67,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) { {{-200000.f, 0.f, 0.f}}}); const PointCloud result = VoxelFilter(point_cloud, 0.01f); EXPECT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); EXPECT_THAT(result.points(), Contains(point_cloud[3])); } From aeb8a10c01aeebc112840a251176556f96a238e4 Mon Sep 17 00:00:00 2001 From: Christian Clauss Date: Mon, 19 Oct 2020 14:38:41 +0200 Subject: [PATCH 70/99] Undefined name: preceding_comments -> preceding_comments.message. (#1720) Signed-off-by: Christian Clauss --- scripts/update_configuration_doc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/update_configuration_doc.py b/scripts/update_configuration_doc.py index 920c466ec2..846859495c 100755 --- a/scripts/update_configuration_doc.py +++ b/scripts/update_configuration_doc.py @@ -179,7 +179,7 @@ def GenerateDocumentation(output_file, root): assert message.name not in output_dict output_dict[message.name] = content if message.preceding_comments: - content.extend(preceding_comments) + content.extend(message.preceding_comments) content.append('') for option_type, option_name, option_comments in message.options: # TODO(whess): For now we exclude InitialTrajectoryPose from the From a9ad813c26053403e40ae7c8dc46590ac06365e2 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 22 Oct 2020 09:24:47 +0200 Subject: [PATCH 71/99] Use intensities in `Submap3D` and `RangeDataInserter3D`. (#1765) `Submap3D` now also stores a pointer to `IntensityHybridGrid`. Adapted `RangeDataInserter3D` to also insert intensities. Signed-off-by: Wolfgang Hess --- .../io/hybrid_grid_points_processor.cc | 3 +- .../mapping/3d/range_data_inserter_3d.cc | 28 +++++++++-- .../mapping/3d/range_data_inserter_3d.h | 7 +-- .../mapping/3d/range_data_inserter_3d_test.cc | 49 +++++++++++++++++-- cartographer/mapping/3d/submap_3d.cc | 13 ++++- cartographer/mapping/3d/submap_3d.h | 9 ++++ .../3d/local_trajectory_builder_3d_test.cc | 1 + .../fast_correlative_scan_matcher_3d_test.cc | 4 +- .../range_data_inserter_options_3d.proto | 3 ++ configuration_files/trajectory_builder_3d.lua | 2 + 10 files changed, 106 insertions(+), 13 deletions(-) diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index a573498096..c81c1d5043 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -40,7 +40,8 @@ HybridGridPointsProcessor::FromDictionary( void HybridGridPointsProcessor::Process(std::unique_ptr batch) { range_data_inserter_.Insert( - {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_); + {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); next_->Process(std::move(batch)); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.cc b/cartographer/mapping/3d/range_data_inserter_3d.cc index 104f805dd9..555323755a 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -51,6 +51,21 @@ void InsertMissesIntoGrid(const std::vector& miss_table, } } +void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns, + IntensityHybridGrid* intensity_hybrid_grid, + const float intensity_threshold) { + if (returns.intensities().size() > 0) { + for (size_t i = 0; i < returns.size(); ++i) { + if (returns.intensities()[i] > intensity_threshold) { + continue; + } + const Eigen::Array3i hit_cell = + intensity_hybrid_grid->GetCellIndex(returns[i].position); + intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]); + } + } +} + } // namespace proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( @@ -62,6 +77,8 @@ proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( parameter_dictionary->GetDouble("miss_probability")); options.set_num_free_space_voxels( parameter_dictionary->GetInt("num_free_space_voxels")); + options.set_intensity_threshold( + parameter_dictionary->GetDouble("intensity_threshold")); CHECK_GT(options.hit_probability(), 0.5); CHECK_LT(options.miss_probability(), 0.5); return options; @@ -75,9 +92,10 @@ RangeDataInserter3D::RangeDataInserter3D( miss_table_( ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {} -void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const { - CHECK(hybrid_grid != nullptr); +void RangeDataInserter3D::Insert( + const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const { + CHECK_NOTNULL(hybrid_grid); for (const sensor::RangefinderPoint& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); @@ -88,6 +106,10 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, // (i.e. no hits will be ignored because of a miss in the same cell). InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, hybrid_grid, options_.num_free_space_voxels()); + if (intensity_hybrid_grid != nullptr) { + InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid, + options_.intensity_threshold()); + } hybrid_grid->FinishUpdate(); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.h b/cartographer/mapping/3d/range_data_inserter_3d.h index d21d2fac5e..0dd5a8e789 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.h +++ b/cartographer/mapping/3d/range_data_inserter_3d.h @@ -36,9 +36,10 @@ class RangeDataInserter3D { RangeDataInserter3D(const RangeDataInserter3D&) = delete; RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete; - // Inserts 'range_data' into 'hybrid_grid'. - void Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const; + // Inserts 'range_data' into 'hybrid_grid' and optionally into + // 'intensity_hybrid_grid'. + void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const; private: const proto::RangeDataInserterOptions3D options_; diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index 0abc75ad38..83dcd4eaeb 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -28,12 +28,13 @@ namespace { class RangeDataInserter3DTest : public ::testing::Test { protected: - RangeDataInserter3DTest() : hybrid_grid_(1.f) { + RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 1000, " + "intensity_threshold = 100, " "}"); options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get()); range_data_inserter_.reset(new RangeDataInserter3D(options_)); @@ -41,14 +42,28 @@ class RangeDataInserter3DTest : public ::testing::Test { void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); - std::vector returns = { + const std::vector returns = { {Eigen::Vector3f{-3.f, -1.f, 4.f}}, {Eigen::Vector3f{-2.f, 0.f, 4.f}}, {Eigen::Vector3f{-1.f, 1.f, 4.f}}, {Eigen::Vector3f{0.f, 2.f, 4.f}}}; range_data_inserter_->Insert( sensor::RangeData{origin, sensor::PointCloud(returns), {}}, - &hybrid_grid_); + &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); + } + + void InsertPointCloudWithIntensities() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + const std::vector intensities{7.f, 8.f, 9.f, 10.f}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}}, + &hybrid_grid_, &intensity_hybrid_grid_); } float GetProbability(float x, float y, float z) const { @@ -56,6 +71,11 @@ class RangeDataInserter3DTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } + float GetIntensity(float x, float y, float z) const { + return intensity_hybrid_grid_.GetIntensity( + intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + float IsKnown(float x, float y, float z) const { return hybrid_grid_.IsKnown( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); @@ -65,6 +85,7 @@ class RangeDataInserter3DTest : public ::testing::Test { private: HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; std::unique_ptr range_data_inserter_; proto::RangeDataInserterOptions3D options_; }; @@ -89,6 +110,28 @@ TEST_F(RangeDataInserter3DTest, InsertPointCloud) { } } +TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) { + InsertPointCloudWithIntensities(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x, y, 4.f)); + EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6); + } else { + EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), + 1e-4); + EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6); + } + } + } +} + TEST_F(RangeDataInserter3DTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 7d75bd94e2..9facbe4a5a 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -202,6 +202,8 @@ Submap3D::Submap3D(const float high_resolution, const float low_resolution, absl::make_unique(high_resolution)), low_resolution_hybrid_grid_( absl::make_unique(low_resolution)), + high_resolution_intensity_hybrid_grid_( + absl::make_unique(high_resolution)), rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {} Submap3D::Submap3D(const proto::Submap3D& proto) @@ -278,9 +280,11 @@ void Submap3D::InsertData(const sensor::RangeData& range_data_in_local, range_data_inserter.Insert( FilterRangeDataByMaxRange(transformed_range_data, high_resolution_max_range), - high_resolution_hybrid_grid_.get()); + high_resolution_hybrid_grid_.get(), + high_resolution_intensity_hybrid_grid_.get()); range_data_inserter.Insert(transformed_range_data, - low_resolution_hybrid_grid_.get()); + low_resolution_hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); set_num_range_data(num_range_data() + 1); const float yaw_in_submap_from_gravity = transform::GetYaw( local_pose().inverse().rotation() * local_from_gravity_aligned); @@ -332,6 +336,11 @@ void ActiveSubmaps3D::AddSubmap( // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. CHECK(submaps_.front()->insertion_finished()); + // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use + // active submaps and their associated intensity hybrid grids for scan + // matching, we call `ForgetIntensityHybridGrid` once we remove the submap + // from active submaps and no longer need the intensity hybrid grid. + submaps_.front()->ForgetIntensityHybridGrid(); submaps_.erase(submaps_.begin()); } const Eigen::VectorXf initial_rotational_scan_matcher_histogram = diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index 5be68cb61e..e5d7e8b443 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -60,6 +60,14 @@ class Submap3D : public Submap { const HybridGrid& low_resolution_hybrid_grid() const { return *low_resolution_hybrid_grid_; } + const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const { + CHECK(high_resolution_intensity_hybrid_grid_ != nullptr); + return *high_resolution_intensity_hybrid_grid_; + } + void ForgetIntensityHybridGrid() { + high_resolution_intensity_hybrid_grid_.reset(); + } + const Eigen::VectorXf& rotational_scan_matcher_histogram() const { return rotational_scan_matcher_histogram_; } @@ -79,6 +87,7 @@ class Submap3D : public Submap { std::unique_ptr high_resolution_hybrid_grid_; std::unique_ptr low_resolution_hybrid_grid_; + std::unique_ptr high_resolution_intensity_hybrid_grid_; Eigen::VectorXf rotational_scan_matcher_histogram_; }; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 381fde2153..e59a1b9c84 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -129,6 +129,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { hit_probability = 0.7, miss_probability = 0.4, num_free_space_voxels = 0, + intensity_threshold = 100.0, }, }, } diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 85ea48ae24..0bcfecef89 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -94,6 +94,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " + "intensity_threshold = 100.0, " "}"); return CreateRangeDataInserterOptions3D(parameter_dictionary.get()); } @@ -106,7 +107,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), {}}, - hybrid_grid_.get()); + hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); hybrid_grid_->FinishUpdate(); return absl::make_unique( diff --git a/cartographer/mapping/proto/range_data_inserter_options_3d.proto b/cartographer/mapping/proto/range_data_inserter_options_3d.proto index 5fcf1985ff..fd5a44a54c 100644 --- a/cartographer/mapping/proto/range_data_inserter_options_3d.proto +++ b/cartographer/mapping/proto/range_data_inserter_options_3d.proto @@ -28,4 +28,7 @@ message RangeDataInserterOptions3D { // Up to how many free space voxels are updated for scan matching. // 0 disables free space. int32 num_free_space_voxels = 3; + + // Do not insert intensities above this threshold into IntensityHybridGrid. + float intensity_threshold = 4; } diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 3bd3dd8eca..6e3d3b615e 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -13,6 +13,7 @@ -- limitations under the License. MAX_3D_RANGE = 60. +INTENSITY_THRESHOLD = 40 TRAJECTORY_BUILDER_3D = { min_range = 1., @@ -96,6 +97,7 @@ TRAJECTORY_BUILDER_3D = { hit_probability = 0.55, miss_probability = 0.49, num_free_space_voxels = 2, + intensity_threshold = INTENSITY_THRESHOLD, }, }, } From 1f69b83e8abc497c3c8207ae7748d1f06e9a82a0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 23 Oct 2020 09:38:58 -0400 Subject: [PATCH 72/99] Minor fixes for Cartographer from ROS (#1705) * restrict boost dependencies to the ones used * Also add googletest as a build dependency. * Set GMOCK_INCLUDE_DIRS when doing src fallback. Signed-off-by: Mikael Arguedas Signed-off-by: Chris Lalancette Co-authored-by: Mikael Arguedas --- cmake/modules/FindGMock.cmake | 1 + package.xml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/modules/FindGMock.cmake b/cmake/modules/FindGMock.cmake index 76cea16161..b29c0fe364 100644 --- a/cmake/modules/FindGMock.cmake +++ b/cmake/modules/FindGMock.cmake @@ -77,6 +77,7 @@ if(NOT GMock_FOUND) EXCLUDE_FROM_ALL) endif() set(GMOCK_LIBRARIES gmock_main) + set(GMOCK_INCLUDE_DIRS ${GMOCK_SRC_DIR}/include) endif() endif() diff --git a/package.xml b/package.xml index 19c811cb09..2d4d317b6f 100644 --- a/package.xml +++ b/package.xml @@ -38,9 +38,10 @@ git google-mock + gtest python3-sphinx - boost + libboost-iostreams-dev eigen libcairo2-dev libceres-dev From e7f98f3e09bfa2b42f8b2ab87f82879d8bd0c293 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 26 Oct 2020 14:47:07 +0100 Subject: [PATCH 73/99] Add intensities to LocalTrajectoryBuilder3D. (#1766) Disabled by default. Signed-off-by: Wolfgang Hess --- .../3d/local_trajectory_builder_3d.cc | 51 +++++++++++++------ .../3d/local_trajectory_builder_3d_test.cc | 2 + .../3d/local_trajectory_builder_options_3d.cc | 1 + .../local_trajectory_builder_options_3d.proto | 5 +- configuration_files/trajectory_builder_3d.lua | 10 ++++ 5 files changed, 52 insertions(+), 17 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index a9bf65f3ae..b2a4f86bc1 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -82,15 +82,18 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( transform::Rigid3d pose_observation_in_submap; ceres::Solver::Summary summary; + const auto* high_resolution_intensity_hybrid_grid = + options_.use_intensities() + ? &matching_submap->high_resolution_intensity_hybrid_grid() + : nullptr; ceres_scan_matcher_->Match( (matching_submap->local_pose().inverse() * pose_prediction).translation(), - initial_ceres_pose, - {{&high_resolution_point_cloud_in_tracking, - &matching_submap->high_resolution_hybrid_grid(), - /*intensity_hybrid_grid=*/nullptr}, - {&low_resolution_point_cloud_in_tracking, - &matching_submap->low_resolution_hybrid_grid(), - /*intensity_hybrid_grid=*/nullptr}}, + initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, + &matching_submap->high_resolution_hybrid_grid(), + high_resolution_intensity_hybrid_grid}, + {&low_resolution_point_cloud_in_tracking, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, &pose_observation_in_submap, &summary); kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance = (pose_observation_in_submap.translation() - @@ -119,7 +122,6 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { initial_imu_data.push_back(sensor::FromProto(imu)); } initial_imu_data.push_back(imu_data); - extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( options_.pose_extrapolator_options(), initial_imu_data, initial_poses); } @@ -128,6 +130,12 @@ std::unique_ptr LocalTrajectoryBuilder3D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { + if (options_.use_intensities()) { + CHECK_EQ(unsynchronized_data.ranges.size(), + unsynchronized_data.intensities.size()) + << "Passed point cloud has inconsistent number of intensities and " + "ranges."; + } auto synchronized_data = range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); if (synchronized_data.ranges.empty()) { @@ -198,7 +206,14 @@ LocalTrajectoryBuilder3D::AddRangeData( hits_poses.push_back(extrapolation_result.current_pose.cast()); CHECK_EQ(hits_poses.size(), hit_times.size()); - sensor::RangeData accumulated_range_data; + const size_t max_possible_number_of_accumulated_points = hit_times.size(); + std::vector accumulated_points; + std::vector accumulated_intensities; + accumulated_points.reserve(max_possible_number_of_accumulated_points); + if (options_.use_intensities()) { + accumulated_intensities.reserve(max_possible_number_of_accumulated_points); + } + sensor::PointCloud misses; std::vector::const_iterator hits_poses_it = hits_poses.begin(); for (const auto& point_cloud_origin_data : @@ -212,13 +227,17 @@ LocalTrajectoryBuilder3D::AddRangeData( const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { - accumulated_range_data.returns.push_back( - sensor::RangefinderPoint{hit_in_local}); + accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local}); + if (options_.use_intensities()) { + accumulated_intensities.push_back(hit.intensity); + } } else { // We insert a ray cropped to 'max_range' as a miss for hits beyond // the maximum range. This way the free space up to the maximum range // will be updated. - accumulated_range_data.misses.push_back(sensor::RangefinderPoint{ + // TODO(wohe): since `misses` are not used anywhere in 3D, consider + // removing `misses` from `range_data` and/or everywhere in 3D. + misses.push_back(sensor::RangefinderPoint{ origin_in_local + options_.max_range() / range * delta}); } } @@ -226,6 +245,8 @@ LocalTrajectoryBuilder3D::AddRangeData( } } CHECK(std::next(hits_poses_it) == hits_poses.end()); + const sensor::PointCloud returns(std::move(accumulated_points), + std::move(accumulated_intensities)); const common::Time current_sensor_time = synchronized_data.time; absl::optional sensor_duration; @@ -238,10 +259,8 @@ LocalTrajectoryBuilder3D::AddRangeData( const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { extrapolation_result.current_pose.translation().cast(), - sensor::VoxelFilter(accumulated_range_data.returns, - options_.voxel_filter_size()), - sensor::VoxelFilter(accumulated_range_data.misses, - options_.voxel_filter_size())}; + sensor::VoxelFilter(returns, options_.voxel_filter_size()), + sensor::VoxelFilter(misses, options_.voxel_filter_size())}; const auto voxel_filter_stop = std::chrono::steady_clock::now(); const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index e59a1b9c84..260264985c 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -132,6 +132,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { intensity_threshold = 100.0, }, }, + + use_intensities = false, } )text"); return mapping::CreateLocalTrajectoryBuilderOptions3D( diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc index 6dd63abdeb..860b7d674f 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -66,6 +66,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetInt("rotational_histogram_size")); *options.mutable_submaps_options() = CreateSubmapsOptions3D( parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_intensities(parameter_dictionary->GetBool("use_intensities")); return options; } diff --git a/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto index f29debd996..2ff762a707 100644 --- a/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto @@ -25,7 +25,7 @@ import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/sensor/proto/sensor.proto"; import "cartographer/transform/proto/timestamped_transform.proto"; -// NEXT ID: 21 +// NEXT ID: 22 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -73,4 +73,7 @@ message LocalTrajectoryBuilderOptions3D { repeated sensor.proto.ImuData initial_imu_data = 20; SubmapsOptions3D submaps_options = 8; + + // Whether to use Lidar intensities in Ceres Scan Matcher. + bool use_intensities = 21; } diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 6e3d3b615e..489889d867 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -44,6 +44,11 @@ TRAJECTORY_BUILDER_3D = { ceres_scan_matcher = { occupied_space_weight_0 = 1., occupied_space_weight_1 = 6., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 0.3, + intensity_threshold = INTENSITY_THRESHOLD, + }, translation_weight = 5., rotation_weight = 4e2, only_optimize_yaw = false, @@ -100,4 +105,9 @@ TRAJECTORY_BUILDER_3D = { intensity_threshold = INTENSITY_THRESHOLD, }, }, + + -- When setting use_intensites to true, the intensity_cost_function_options_0 + -- parameter in ceres_scan_matcher has to be set up as well or otherwise + -- CeresScanMatcher will CHECK-fail. + use_intensities = false, } From 4d90a298772d1028fe9f08b429448197da0f2466 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 4 Nov 2020 12:16:51 +0100 Subject: [PATCH 74/99] Make unusable public headers internal. (#1772) pose_graph_data.h and io test_helpers.h are currently public, i.e. get installed, but cannot possibly be used since they refer to internal headers. Signed-off-by: Wolfgang Hess --- cartographer/cloud/internal/client_server_test.cc | 2 +- cartographer/io/{ => internal}/testing/test_helpers.cc | 2 +- cartographer/io/{ => internal}/testing/test_helpers.h | 0 cartographer/io/proto_stream_deserializer_test.cc | 2 +- cartographer/mapping/internal/2d/pose_graph_2d.h | 2 +- cartographer/mapping/internal/3d/pose_graph_3d.h | 2 +- cartographer/mapping/{ => internal}/pose_graph_data.h | 0 7 files changed, 5 insertions(+), 5 deletions(-) rename cartographer/io/{ => internal}/testing/test_helpers.cc (96%) rename cartographer/io/{ => internal}/testing/test_helpers.h (100%) rename cartographer/mapping/{ => internal}/pose_graph_data.h (100%) diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 8a089710b3..9d8bc38bee 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -21,9 +21,9 @@ #include "cartographer/cloud/internal/map_builder_server.h" #include "cartographer/cloud/map_builder_server_options.h" #include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/io/internal/testing/test_helpers.h" #include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream_deserializer.h" -#include "cartographer/io/testing/test_helpers.h" #include "cartographer/mapping/internal/testing/mock_map_builder.h" #include "cartographer/mapping/internal/testing/mock_pose_graph.h" #include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" diff --git a/cartographer/io/testing/test_helpers.cc b/cartographer/io/internal/testing/test_helpers.cc similarity index 96% rename from cartographer/io/testing/test_helpers.cc rename to cartographer/io/internal/testing/test_helpers.cc index 00fcf57931..96a56d5bc7 100644 --- a/cartographer/io/testing/test_helpers.cc +++ b/cartographer/io/internal/testing/test_helpers.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/io/testing/test_helpers.h" +#include "cartographer/io/internal/testing/test_helpers.h" #include "cartographer/mapping/proto/serialization.pb.h" diff --git a/cartographer/io/testing/test_helpers.h b/cartographer/io/internal/testing/test_helpers.h similarity index 100% rename from cartographer/io/testing/test_helpers.h rename to cartographer/io/internal/testing/test_helpers.h diff --git a/cartographer/io/proto_stream_deserializer_test.cc b/cartographer/io/proto_stream_deserializer_test.cc index d221a25f8b..90797991c9 100644 --- a/cartographer/io/proto_stream_deserializer_test.cc +++ b/cartographer/io/proto_stream_deserializer_test.cc @@ -19,7 +19,7 @@ #include #include "cartographer/io/internal/in_memory_proto_stream.h" -#include "cartographer/io/testing/test_helpers.h" +#include "cartographer/io/internal/testing/test_helpers.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index bf20f42381..373342525f 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -35,10 +35,10 @@ #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#include "cartographer/mapping/internal/pose_graph_data.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/work_queue.h" #include "cartographer/mapping/pose_graph.h" -#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/value_conversion_tables.h" #include "cartographer/metrics/family_factory.h" diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index f8467aacdd..12a3469a72 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -36,9 +36,9 @@ #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" +#include "cartographer/mapping/internal/pose_graph_data.h" #include "cartographer/mapping/internal/work_queue.h" #include "cartographer/mapping/pose_graph.h" -#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/fixed_frame_pose_data.h" diff --git a/cartographer/mapping/pose_graph_data.h b/cartographer/mapping/internal/pose_graph_data.h similarity index 100% rename from cartographer/mapping/pose_graph_data.h rename to cartographer/mapping/internal/pose_graph_data.h From 6715afe70c70753767c1510e0a35955c0af6cc3e Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 4 Nov 2020 14:55:13 +0100 Subject: [PATCH 75/99] Optional motion-filter for adding odometry data to the pose graph. (#1757) This introduces an option to control the amount of odometry data that enters the pose graph by motion-filtering it. This is very useful to bound memory when standing still for longer periods of time. If the new option is not configured, all odometry data goes unfiltered into the pose graph as usual. Signed-off-by: Michael Grupp --- .../internal/global_trajectory_builder.cc | 27 ++++++++++++++----- .../internal/global_trajectory_builder.h | 6 +++-- cartographer/mapping/map_builder.cc | 14 ++++++++-- .../proto/trajectory_builder_options.proto | 3 +++ .../mapping/trajectory_builder_interface.cc | 18 +++++++++++++ 5 files changed, 58 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index 3260f05bfe..f68645f14a 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -19,7 +19,9 @@ #include #include "absl/memory/memory.h" +#include "absl/types/optional.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/metrics/family_factory.h" #include "glog/logging.h" @@ -39,11 +41,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { GlobalTrajectoryBuilder( std::unique_ptr local_trajectory_builder, const int trajectory_id, PoseGraph* const pose_graph, - const LocalSlamResultCallback& local_slam_result_callback) + const LocalSlamResultCallback& local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) : trajectory_id_(trajectory_id), pose_graph_(pose_graph), local_trajectory_builder_(std::move(local_trajectory_builder)), - local_slam_result_callback_(local_slam_result_callback) {} + local_slam_result_callback_(local_slam_result_callback), + pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {} ~GlobalTrajectoryBuilder() override {} GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; @@ -97,6 +101,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { if (local_trajectory_builder_) { local_trajectory_builder_->AddOdometryData(odometry_data); } + // TODO(MichaelGrupp): Instead of having an optional filter on this level, + // odometry could be marginalized between nodes in the pose graph. + // Related issue: cartographer-project/cartographer/#1768 + if (pose_graph_odometry_motion_filter_.has_value() && + pose_graph_odometry_motion_filter_.value().IsSimilar( + odometry_data.time, odometry_data.pose)) { + return; + } pose_graph_->AddOdometryData(trajectory_id_, odometry_data); } @@ -127,6 +139,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { PoseGraph* const pose_graph_; std::unique_ptr local_trajectory_builder_; LocalSlamResultCallback local_slam_result_callback_; + absl::optional pose_graph_odometry_motion_filter_; }; } // namespace @@ -135,22 +148,24 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback) { + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, - local_slam_result_callback); + local_slam_result_callback, pose_graph_odometry_motion_filter); } std::unique_ptr CreateGlobalTrajectoryBuilder3D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback) { + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, - local_slam_result_callback); + local_slam_result_callback, pose_graph_odometry_motion_filter); } void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory* factory) { diff --git a/cartographer/mapping/internal/global_trajectory_builder.h b/cartographer/mapping/internal/global_trajectory_builder.h index 754912d59b..46694367a2 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.h +++ b/cartographer/mapping/internal/global_trajectory_builder.h @@ -34,13 +34,15 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback); + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); std::unique_ptr CreateGlobalTrajectoryBuilder3D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback); + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); void GlobalTrajectoryBuilderRegisterMetrics( metrics::FamilyFactory* family_factory); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 7809e6f262..208d9e246d 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/map_builder.h" #include "absl/memory/memory.h" +#include "absl/types/optional.h" #include "cartographer/common/time.h" #include "cartographer/io/internal/mapping_state_serialization.h" #include "cartographer/io/proto_stream.h" @@ -28,6 +29,7 @@ #include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/collated_trajectory_builder.h" #include "cartographer/mapping/internal/global_trajectory_builder.h" +#include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/sensor/internal/collator.h" #include "cartographer/sensor/internal/trajectory_collator.h" #include "cartographer/sensor/internal/voxel_filter.h" @@ -120,6 +122,14 @@ int MapBuilder::AddTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) { const int trajectory_id = trajectory_builders_.size(); + + absl::optional pose_graph_odometry_motion_filter; + if (trajectory_options.has_pose_graph_odometry_motion_filter()) { + LOG(INFO) << "Using a motion filter for adding odometry to the pose graph."; + pose_graph_odometry_motion_filter.emplace( + MotionFilter(trajectory_options.pose_graph_odometry_motion_filter())); + } + if (options_.use_trajectory_builder_3d()) { std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_3d_options()) { @@ -134,7 +144,7 @@ int MapBuilder::AddTrajectoryBuilder( CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } else { std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { @@ -149,7 +159,7 @@ int MapBuilder::AddTrajectoryBuilder( CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get()); diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index fe20db05d3..13f034470a 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -15,6 +15,7 @@ syntax = "proto3"; import "cartographer/transform/proto/transform.proto"; +import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto"; import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto"; @@ -41,6 +42,8 @@ message TrajectoryBuilderOptions { bool collate_fixed_frame = 7; bool collate_landmarks = 8; + + MotionFilterOptions pose_graph_odometry_motion_filter = 9; } message SensorId { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 8ed8377f11..991dcc4dca 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -37,6 +37,23 @@ void PopulatePureLocalizationTrimmerOptions( options_dictionary->GetInt("max_submaps_to_keep")); } +void PopulatePoseGraphOdometryMotionFilterOptions( + proto::TrajectoryBuilderOptions* const trajectory_builder_options, + common::LuaParameterDictionary* const parameter_dictionary) { + constexpr char kDictionaryKey[] = "pose_graph_odometry_motion_filter"; + if (!parameter_dictionary->HasKey(kDictionaryKey)) return; + + auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey); + auto* options = + trajectory_builder_options->mutable_pose_graph_odometry_motion_filter(); + options->set_max_time_seconds( + options_dictionary->GetDouble("max_time_seconds")); + options->set_max_distance_meters( + options_dictionary->GetDouble("max_distance_meters")); + options->set_max_angle_radians( + options_dictionary->GetDouble("max_angle_radians")); +} + } // namespace proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( @@ -53,6 +70,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( options.set_collate_landmarks( parameter_dictionary->GetBool("collate_landmarks")); PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary); + PopulatePoseGraphOdometryMotionFilterOptions(&options, parameter_dictionary); return options; } From 38dcf65d8fd586249fce5a2aeed8dde45d54f91a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 4 Nov 2020 16:35:02 +0100 Subject: [PATCH 76/99] Clean up the public headers in `common`. (#1775) In common, BlockingQueue and RateTimer are not used in any public API and are made internal. `common/utils.h` is unused code and removed. Signed-off-by: Wolfgang Hess --- .../internal/local_trajectory_uploader.cc | 2 +- .../internal/map_builder_context_interface.h | 2 +- .../cloud/internal/map_builder_server.h | 2 +- .../common/{ => internal}/blocking_queue.h | 0 .../{ => internal}/blocking_queue_test.cc | 2 +- .../common/{ => internal}/rate_timer.h | 0 .../common/{ => internal}/rate_timer_test.cc | 2 +- cartographer/common/utils.h | 34 ------------------- .../internal/collated_trajectory_builder.h | 2 +- .../sensor/internal/ordered_multi_queue.h | 2 +- 10 files changed, 7 insertions(+), 41 deletions(-) rename cartographer/common/{ => internal}/blocking_queue.h (100%) rename cartographer/common/{ => internal}/blocking_queue_test.cc (98%) rename cartographer/common/{ => internal}/rate_timer.h (100%) rename cartographer/common/{ => internal}/rate_timer_test.cc (97%) delete mode 100644 cartographer/common/utils.h diff --git a/cartographer/cloud/internal/local_trajectory_uploader.cc b/cartographer/cloud/internal/local_trajectory_uploader.cc index 4a0dc692a7..3f5a160451 100644 --- a/cartographer/cloud/internal/local_trajectory_uploader.cc +++ b/cartographer/cloud/internal/local_trajectory_uploader.cc @@ -25,7 +25,7 @@ #include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" #include "cartographer/cloud/internal/sensor/serialization.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "glog/logging.h" #include "grpc++/grpc++.h" diff --git a/cartographer/cloud/internal/map_builder_context_interface.h b/cartographer/cloud/internal/map_builder_context_interface.h index ee6fe2d466..bbcd11132c 100644 --- a/cartographer/cloud/internal/map_builder_context_interface.h +++ b/cartographer/cloud/internal/map_builder_context_interface.h @@ -19,7 +19,7 @@ #include "async_grpc/execution_context.h" #include "cartographer/cloud/internal/local_trajectory_uploader.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/serialization.pb.h" diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index 83c7e3c1fd..279dd2fbb2 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/cartographer/cloud/internal/map_builder_server.h @@ -23,7 +23,7 @@ #include "cartographer/cloud/internal/map_builder_context_interface.h" #include "cartographer/cloud/map_builder_server_interface.h" #include "cartographer/cloud/proto/map_builder_server_options.pb.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/common/time.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/3d/submap_3d.h" diff --git a/cartographer/common/blocking_queue.h b/cartographer/common/internal/blocking_queue.h similarity index 100% rename from cartographer/common/blocking_queue.h rename to cartographer/common/internal/blocking_queue.h diff --git a/cartographer/common/blocking_queue_test.cc b/cartographer/common/internal/blocking_queue_test.cc similarity index 98% rename from cartographer/common/blocking_queue_test.cc rename to cartographer/common/internal/blocking_queue_test.cc index b6f019f5c6..47f961e0e8 100644 --- a/cartographer/common/blocking_queue_test.cc +++ b/cartographer/common/internal/blocking_queue_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include #include diff --git a/cartographer/common/rate_timer.h b/cartographer/common/internal/rate_timer.h similarity index 100% rename from cartographer/common/rate_timer.h rename to cartographer/common/internal/rate_timer.h diff --git a/cartographer/common/rate_timer_test.cc b/cartographer/common/internal/rate_timer_test.cc similarity index 97% rename from cartographer/common/rate_timer_test.cc rename to cartographer/common/internal/rate_timer_test.cc index 9bf3c753ec..8c73bddd54 100644 --- a/cartographer/common/rate_timer_test.cc +++ b/cartographer/common/internal/rate_timer_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/rate_timer.h" +#include "cartographer/common/internal/rate_timer.h" #include "gtest/gtest.h" diff --git a/cartographer/common/utils.h b/cartographer/common/utils.h deleted file mode 100644 index 229ad694f1..0000000000 --- a/cartographer/common/utils.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * 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_COMMON_UTILS_H_ -#define CARTOGRAPHER_COMMON_UTILS_H_ - -namespace cartographer { -namespace common { - -template -ValueType* FindOrNull(MapType& map, const KeyType& key) { - auto it = map.find(key); - if (it == map.end()) return nullptr; - return &(it->second); -} - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_UTILS_H_ diff --git a/cartographer/mapping/internal/collated_trajectory_builder.h b/cartographer/mapping/internal/collated_trajectory_builder.h index c999edfaf0..e9e39ffc01 100644 --- a/cartographer/mapping/internal/collated_trajectory_builder.h +++ b/cartographer/mapping/internal/collated_trajectory_builder.h @@ -23,8 +23,8 @@ #include #include +#include "cartographer/common/internal/rate_timer.h" #include "cartographer/common/port.h" -#include "cartographer/common/rate_timer.h" #include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder_interface.h" diff --git a/cartographer/sensor/internal/ordered_multi_queue.h b/cartographer/sensor/internal/ordered_multi_queue.h index 36f17e371a..d4849863b8 100644 --- a/cartographer/sensor/internal/ordered_multi_queue.h +++ b/cartographer/sensor/internal/ordered_multi_queue.h @@ -23,7 +23,7 @@ #include #include -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/sensor/internal/dispatchable.h" From b1ce24d3f8e0b2a372467d65dc92d6c52329bb90 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 6 Nov 2020 11:31:49 +0100 Subject: [PATCH 77/99] Clean up use of MapBuilderInterface. (#1776) Moves options next to the interface like we do for other interfaces. Adds a factory function to remove the need for direct use of MapBuilder. Signed-off-by: Wolfgang Hess --- .../cloud/internal/client_server_test.cc | 9 ++-- .../cloud/internal/map_builder_server.h | 1 - cartographer/cloud/map_builder_server_main.cc | 2 +- .../cloud/map_builder_server_options.cc | 2 +- .../common/configuration_files_test.cc | 2 +- .../io/serialization_format_migration.cc | 1 - cartographer/mapping/map_builder.cc | 23 +++------- cartographer/mapping/map_builder.h | 6 +-- cartographer/mapping/map_builder_interface.cc | 43 +++++++++++++++++++ cartographer/mapping/map_builder_interface.h | 4 ++ cartographer/mapping/map_builder_test.cc | 2 +- 11 files changed, 64 insertions(+), 31 deletions(-) create mode 100644 cartographer/mapping/map_builder_interface.cc diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 9d8bc38bee..1eaea52e15 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -29,12 +29,13 @@ #include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" #include "cartographer/mapping/internal/testing/test_helpers.h" #include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/map_builder.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "gtest/gtest.h" using ::cartographer::io::testing::ProtoReaderFromStrings; -using ::cartographer::mapping::MapBuilder; +using ::cartographer::mapping::CreateMapBuilder; using ::cartographer::mapping::MapBuilderInterface; using ::cartographer::mapping::PoseGraphInterface; using ::cartographer::mapping::TrajectoryBuilderInterface; @@ -139,15 +140,15 @@ class ClientServerTestBase : public T { } void InitializeRealServer() { - auto map_builder = absl::make_unique( - map_builder_server_options_.map_builder_options()); + auto map_builder = + CreateMapBuilder(map_builder_server_options_.map_builder_options()); server_ = absl::make_unique(map_builder_server_options_, std::move(map_builder)); EXPECT_TRUE(server_ != nullptr); } void InitializeRealUploadingServer() { - auto map_builder = absl::make_unique( + auto map_builder = CreateMapBuilder( uploading_map_builder_server_options_.map_builder_options()); uploading_server_ = absl::make_unique( uploading_map_builder_server_options_, std::move(map_builder)); diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index 279dd2fbb2..ed7594362b 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/cartographer/cloud/internal/map_builder_server.h @@ -29,7 +29,6 @@ #include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/internal/submap_controller.h" #include "cartographer/mapping/local_slam_result_data.h" -#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/internal/dispatchable.h" diff --git a/cartographer/cloud/map_builder_server_main.cc b/cartographer/cloud/map_builder_server_main.cc index 67d4051556..81bf87eca0 100644 --- a/cartographer/cloud/map_builder_server_main.cc +++ b/cartographer/cloud/map_builder_server_main.cc @@ -50,7 +50,7 @@ void Run(const std::string& configuration_directory, proto::MapBuilderServerOptions map_builder_server_options = LoadMapBuilderServerOptions(configuration_directory, configuration_basename); - auto map_builder = absl::make_unique( + auto map_builder = mapping::CreateMapBuilder( map_builder_server_options.map_builder_options()); std::unique_ptr map_builder_server = CreateMapBuilderServer(map_builder_server_options, diff --git a/cartographer/cloud/map_builder_server_options.cc b/cartographer/cloud/map_builder_server_options.cc index 25fcc87722..84042f3224 100644 --- a/cartographer/cloud/map_builder_server_options.cc +++ b/cartographer/cloud/map_builder_server_options.cc @@ -18,7 +18,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" namespace cartographer { namespace cloud { diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc index 62db6d8f48..d3c451abb0 100644 --- a/cartographer/common/configuration_files_test.cc +++ b/cartographer/common/configuration_files_test.cc @@ -20,7 +20,7 @@ #include "cartographer/common/config.h" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc index fb3c222ce7..7ed692e36f 100644 --- a/cartographer/io/serialization_format_migration.cc +++ b/cartographer/io/serialization_format_migration.cc @@ -28,7 +28,6 @@ #include "cartographer/mapping/id.h" #include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" -#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/probability_values.h" diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 208d9e246d..5621dbc759 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -74,24 +74,6 @@ void MaybeAddPureLocalizationTrimmer( } // namespace -proto::MapBuilderOptions CreateMapBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::MapBuilderOptions options; - options.set_use_trajectory_builder_2d( - parameter_dictionary->GetBool("use_trajectory_builder_2d")); - options.set_use_trajectory_builder_3d( - parameter_dictionary->GetBool("use_trajectory_builder_3d")); - options.set_num_background_threads( - parameter_dictionary->GetNonNegativeInt("num_background_threads")); - options.set_collate_by_trajectory( - parameter_dictionary->GetBool("collate_by_trajectory")); - *options.mutable_pose_graph_options() = CreatePoseGraphOptions( - parameter_dictionary->GetDictionary("pose_graph").get()); - CHECK_NE(options.use_trajectory_builder_2d(), - options.use_trajectory_builder_3d()); - return options; -} - MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { CHECK(options.use_trajectory_builder_2d() ^ @@ -414,5 +396,10 @@ std::map MapBuilder::LoadStateFromFile( return LoadState(&stream, load_frozen_state); } +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options) { + return absl::make_unique(options); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 8e6db5de2c..ee885a062a 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -28,9 +28,6 @@ namespace cartographer { namespace mapping { -proto::MapBuilderOptions CreateMapBuilderOptions( - common::LuaParameterDictionary *const parameter_dictionary); - // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // and a PoseGraph for loop closure. class MapBuilder : public MapBuilderInterface { @@ -98,6 +95,9 @@ class MapBuilder : public MapBuilderInterface { all_trajectory_builder_options_; }; +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options); + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder_interface.cc b/cartographer/mapping/map_builder_interface.cc new file mode 100644 index 0000000000..af75f6ccb0 --- /dev/null +++ b/cartographer/mapping/map_builder_interface.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2016 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/mapping/map_builder_interface.h" + +#include "cartographer/mapping/pose_graph.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::MapBuilderOptions options; + options.set_use_trajectory_builder_2d( + parameter_dictionary->GetBool("use_trajectory_builder_2d")); + options.set_use_trajectory_builder_3d( + parameter_dictionary->GetBool("use_trajectory_builder_3d")); + options.set_num_background_threads( + parameter_dictionary->GetNonNegativeInt("num_background_threads")); + options.set_collate_by_trajectory( + parameter_dictionary->GetBool("collate_by_trajectory")); + *options.mutable_pose_graph_options() = CreatePoseGraphOptions( + parameter_dictionary->GetDictionary("pose_graph").get()); + CHECK_NE(options.use_trajectory_builder_2d(), + options.use_trajectory_builder_3d()); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 8c1cfb144b..76062fb165 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -27,6 +27,7 @@ #include "cartographer/io/proto_stream_interface.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" @@ -35,6 +36,9 @@ namespace cartographer { namespace mapping { +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + // This interface is used for both library and RPC implementations. // Implementations wire up the complete SLAM stack. class MapBuilderInterface { diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index c079775110..4f205c13dc 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -64,7 +64,7 @@ class MapBuilderTestBase : public T { } void BuildMapBuilder() { - map_builder_ = absl::make_unique(map_builder_options_); + map_builder_ = CreateMapBuilder(map_builder_options_); } void SetOptionsTo3D() { From dddb2f110252b6243a007130279461e35f29eff8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 17 Nov 2020 10:56:38 +0100 Subject: [PATCH 78/99] Improve CONTRIBUTING.md and the pull request template. (#1780) We refer to CONTRIBUTING.md from the PR template now which is now the single place containing all information. Reference to the bot is removed since it is currently not being used. We point contributors to using clang-format. We link to PR and code review best practices now. Signed-off-by: Wolfgang Hess --- .github/PULL_REQUEST_TEMPLATE.md | 16 ++-------------- CONTRIBUTING.md | 23 ++++++++++++++++++++++- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index babe3c9383..ce73cbdeb3 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,14 +1,2 @@ -Want to contribute? Great! Make sure you've read, understood and considered all -the points below before creating your PR: - -- Keep your PR under 200 lines of code and address a single concern. -- Add unit test(s) and documentation (these do not count toward your 200 lines). -- Adhere to the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). -- Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate. -- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis - runs for every update which blocks merging of other changes. Our merge bot - will rebase your branch, reformat your source code and merge as the last step - in the review process. -- Please replace this template text with the commit message you want for your - PR. You and/or the reviewer should keep it updated during the course of the - review using the GitHub edit feature. +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 35894fc397..8495b5217e 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -27,4 +27,25 @@ You can sign-off a commit via `git commit -s`. ### Code reviews All submissions, including submissions by project members, require review. -We use GitHub pull requests for this purpose. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. + +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. From 605838855fe84c30b788c884a419aaa509493cad Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 18 Nov 2020 16:06:53 +0100 Subject: [PATCH 79/99] Removed unused code. (#1781) Signed-off-by: Wolfgang Hess --- .../cloud/internal/testing/handler_test.h | 4 +- cartographer/testing/test_helpers.h | 51 ------------------- 2 files changed, 2 insertions(+), 53 deletions(-) delete mode 100644 cartographer/testing/test_helpers.h diff --git a/cartographer/cloud/internal/testing/handler_test.h b/cartographer/cloud/internal/testing/handler_test.h index 189a70dbfb..b114a6a5bd 100644 --- a/cartographer/cloud/internal/testing/handler_test.h +++ b/cartographer/cloud/internal/testing/handler_test.h @@ -19,11 +19,11 @@ #include "absl/memory/memory.h" #include "async_grpc/testing/rpc_handler_test_server.h" +#include "cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h" +#include "cartographer/cloud/internal/testing/mock_map_builder_context.h" #include "cartographer/mapping/internal/testing/mock_map_builder.h" #include "cartographer/mapping/internal/testing/mock_pose_graph.h" #include "gtest/gtest.h" -#include "mock_local_trajectory_uploader.h" -#include "mock_map_builder_context.h" namespace cartographer { namespace cloud { diff --git a/cartographer/testing/test_helpers.h b/cartographer/testing/test_helpers.h deleted file mode 100644 index 4a7dbdfd19..0000000000 --- a/cartographer/testing/test_helpers.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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_TESTING_TEST_HELPERS_H_ -#define CARTOGRAPHER_TESTING_TEST_HELPERS_H_ - -#include "gmock/gmock.h" -#include "google/protobuf/text_format.h" -#include "google/protobuf/util/message_differencer.h" - -namespace cartographer { -namespace testing { - -template -ProtoType ParseProto(const std::string& proto_string) { - ProtoType proto; - EXPECT_TRUE( - ::google::protobuf::TextFormat::ParseFromString(proto_string, &proto)); - return proto; -} - -MATCHER_P(EqualsProto, expected_proto_string, "") { - using ConstProtoType = typename std::remove_reference::type; - using ProtoType = typename std::remove_cv::type; - - return google::protobuf::util::MessageDifferencer::Equals( - arg, ParseProto(expected_proto_string)); -} - -::testing::Matcher Near(double expected) { - constexpr double kPrecision = 1e-05; - return ::testing::DoubleNear(expected, kPrecision); -} - -} // namespace testing -} // namespace cartographer - -#endif // CARTOGRAPHER_TESTING_TEST_HELPERS_H_ From fdcf0eb0df72b2eb7ae30c08c873b5ab2b855239 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 23 Nov 2020 11:02:39 +0100 Subject: [PATCH 80/99] Move PoseGraph::GetSubmapData to the PoseGraphInterface. (#1783) Related to #1413. Signed-off-by: Wolfgang Hess --- cartographer/cloud/internal/client/pose_graph_stub.cc | 5 +++++ cartographer/cloud/internal/client/pose_graph_stub.h | 1 + cartographer/mapping/internal/testing/mock_pose_graph.h | 1 + cartographer/mapping/pose_graph.h | 5 ----- cartographer/mapping/pose_graph_interface.h | 5 +++++ 5 files changed, 12 insertions(+), 5 deletions(-) diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index f7ff9ad14d..23cacb2e93 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -52,6 +52,11 @@ PoseGraphStub::GetAllSubmapData() const { LOG(FATAL) << "Not implemented"; } +mapping::PoseGraphInterface::SubmapData PoseGraphStub::GetSubmapData( + const mapping::SubmapId& submap_id) const { + LOG(FATAL) << "Not implemented"; +} + mapping::MapById PoseGraphStub::GetAllSubmapPoses() const { google::protobuf::Empty request; diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index b2e6550867..bcca67c11f 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -34,6 +34,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { void RunFinalOptimization() override; mapping::MapById GetAllSubmapData() const override; + SubmapData GetSubmapData(const mapping::SubmapId& submap_id) const override; mapping::MapById GetAllSubmapPoses() const override; transform::Rigid3d GetLocalToGlobalTransform( diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 074ac09735..dbddfd1f79 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -34,6 +34,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface { MOCK_METHOD0(RunFinalOptimization, void()); MOCK_CONST_METHOD0(GetAllSubmapData, mapping::MapById()); + MOCK_CONST_METHOD1(GetSubmapData, SubmapData(const SubmapId&)); MOCK_CONST_METHOD0(GetAllSubmapPoses, mapping::MapById()); MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int)); diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index d6597b0e0c..b4dfd3bbec 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -112,11 +112,6 @@ class PoseGraph : public PoseGraphInterface { // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() const = 0; - // Returns the current optimized transform and submap itself for the given - // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does - // not exist (anymore). - virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; - proto::PoseGraph ToProto(bool include_unfinished_submaps) const override; // Returns the IMU data. diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 2701e93d46..68551f1579 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -99,6 +99,11 @@ class PoseGraphInterface { // Returns data for all submaps. virtual MapById GetAllSubmapData() const = 0; + // Returns the current optimized transform and submap itself for the given + // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does + // not exist (anymore). + virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; + // Returns the global poses for all submaps. virtual MapById GetAllSubmapPoses() const = 0; From 5b5f59bab6004bb7176890d3b907c5e5ab105fdd Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 11 Jan 2021 13:46:33 +0100 Subject: [PATCH 81/99] Move more headers from mapping/ to internal. (#1796) The moved headers are not used in any public API. Signed-off-by: Wolfgang Hess --- cartographer/mapping/2d/submap_2d.cc | 2 +- cartographer/mapping/imu_tracker.cc | 2 +- cartographer/mapping/imu_tracker_test.cc | 2 +- .../real_time_correlative_scan_matcher_2d_test.cc | 2 +- .../2d/scan_matching/tsdf_match_cost_function_2d_test.cc | 2 +- .../mapping/{ => internal}/2d/tsdf_range_data_inserter_2d.cc | 2 +- .../mapping/{ => internal}/2d/tsdf_range_data_inserter_2d.h | 0 .../{ => internal}/2d/tsdf_range_data_inserter_2d_test.cc | 2 +- .../{ => internal}/eigen_quaterniond_from_two_vectors.cc | 2 +- .../{ => internal}/eigen_quaterniond_from_two_vectors.h | 0 .../mapping/{ => internal}/imu_based_pose_extrapolator.cc | 4 ++-- .../mapping/{ => internal}/imu_based_pose_extrapolator.h | 0 cartographer/mapping/pose_extrapolator_interface.cc | 2 +- cartographer/mapping/pose_extrapolator_test.cc | 2 +- cartographer/mapping/range_data_inserter_interface.cc | 4 ++-- 15 files changed, 14 insertions(+), 14 deletions(-) rename cartographer/mapping/{ => internal}/2d/tsdf_range_data_inserter_2d.cc (99%) rename cartographer/mapping/{ => internal}/2d/tsdf_range_data_inserter_2d.h (100%) rename cartographer/mapping/{ => internal}/2d/tsdf_range_data_inserter_2d_test.cc (99%) rename cartographer/mapping/{ => internal}/eigen_quaterniond_from_two_vectors.cc (91%) rename cartographer/mapping/{ => internal}/eigen_quaterniond_from_two_vectors.h (100%) rename cartographer/mapping/{ => internal}/imu_based_pose_extrapolator.cc (99%) rename cartographer/mapping/{ => internal}/imu_based_pose_extrapolator.h (100%) diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index feb30fc22f..a84d48eadd 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -26,7 +26,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "glog/logging.h" diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc index 45391d00bf..8aed29cd0a 100644 --- a/cartographer/mapping/imu_tracker.cc +++ b/cartographer/mapping/imu_tracker.cc @@ -20,7 +20,7 @@ #include #include "cartographer/common/math.h" -#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/imu_tracker_test.cc b/cartographer/mapping/imu_tracker_test.cc index 88ae846221..10992eda62 100644 --- a/cartographer/mapping/imu_tracker_test.cc +++ b/cartographer/mapping/imu_tracker_test.cc @@ -17,7 +17,7 @@ #include "cartographer/mapping/imu_tracker.h" #include "absl/memory/memory.h" -#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index f8eef22fd9..27df5a758b 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -25,7 +25,7 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index 3311ace09f..04c6f05aec 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -19,7 +19,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc index 7a6a4e7cc6..7e6bd3f1a2 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/2d/normal_estimation_2d.h" #include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h similarity index 100% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d.h rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc index 7e59508bda..da06a893b3 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc similarity index 91% rename from cartographer/mapping/eigen_quaterniond_from_two_vectors.cc rename to cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc index af0aac7f94..e0edcb72f6 100644 --- a/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc +++ b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.h b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h similarity index 100% rename from cartographer/mapping/eigen_quaterniond_from_two_vectors.h rename to cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h diff --git a/cartographer/mapping/imu_based_pose_extrapolator.cc b/cartographer/mapping/internal/imu_based_pose_extrapolator.cc similarity index 99% rename from cartographer/mapping/imu_based_pose_extrapolator.cc rename to cartographer/mapping/internal/imu_based_pose_extrapolator.cc index 6c9d72f27b..ddb88ea902 100644 --- a/cartographer/mapping/imu_based_pose_extrapolator.cc +++ b/cartographer/mapping/internal/imu_based_pose_extrapolator.cc @@ -14,14 +14,14 @@ * limitations under the License. */ -#include "cartographer/mapping/imu_based_pose_extrapolator.h" +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" #include #include "absl/memory/memory.h" -#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "cartographer/mapping/internal/3d/imu_integration.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h" #include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" #include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" diff --git a/cartographer/mapping/imu_based_pose_extrapolator.h b/cartographer/mapping/internal/imu_based_pose_extrapolator.h similarity index 100% rename from cartographer/mapping/imu_based_pose_extrapolator.h rename to cartographer/mapping/internal/imu_based_pose_extrapolator.h diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index 413a8da442..b0b504f1b6 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -18,7 +18,7 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/imu_based_pose_extrapolator.h" +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h" namespace cartographer { diff --git a/cartographer/mapping/pose_extrapolator_test.cc b/cartographer/mapping/pose_extrapolator_test.cc index 38afc84a9c..00ef86fa7b 100644 --- a/cartographer/mapping/pose_extrapolator_test.cc +++ b/cartographer/mapping/pose_extrapolator_test.cc @@ -18,7 +18,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" -#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/range_data_inserter_interface.cc b/cartographer/mapping/range_data_inserter_interface.cc index 79d8c8d1bd..571987c711 100644 --- a/cartographer/mapping/range_data_inserter_interface.cc +++ b/cartographer/mapping/range_data_inserter_interface.cc @@ -17,7 +17,7 @@ #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" namespace cartographer { namespace mapping { @@ -46,4 +46,4 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions( return options; } } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer From 802e9f131b9d87d16ef53e4105adc6a975eb2d36 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 19 Jan 2021 19:44:18 +0100 Subject: [PATCH 82/99] Clean trimming of frozen trajectory data. (#1767) The deletion logic needs to take care of deleting all data that is "exclusively" connected to the submaps that are to be removed. This is achieved by looking at the data that is connected via constraints in the graph. Deleting a frozen trajectory (one without optimization constraints) doesn't work that way and would leave dangling nodes in the graph. This adds an additional logic that uses the `node_ids` field of the submap instead of the constraints if the trajectory is frozen. Signed-off-by: Michael Grupp --- .../mapping/internal/2d/pose_graph_2d.cc | 33 ++++++------ .../mapping/internal/3d/pose_graph_3d.cc | 33 ++++++------ .../mapping/internal/3d/pose_graph_3d_test.cc | 53 +++++++++++++++++++ cartographer/mapping/map_builder.cc | 2 +- 4 files changed, 90 insertions(+), 31 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index c533a0d355..060277eb56 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -1210,33 +1210,36 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished); - // Compile all nodes that are still INTRA_SUBMAP constrained once the submap - // with 'submap_id' is gone. + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. std::set nodes_to_retain; - for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - constraint.submap_id != submap_id) { - nodes_to_retain.insert(constraint.node_id); + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); } } - // Remove all 'data_.constraints' related to 'submap_id'. + + // Remove all nodes that are exlusively associated to 'submap_id'. std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. { std::vector constraints; for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.submap_id == submap_id) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - nodes_to_retain.count(constraint.node_id) == 0) { - // This node will no longer be INTRA_SUBMAP contrained and has to be - // removed. - nodes_to_remove.insert(constraint.node_id); - } - } else { + if (constraint.submap_id != submap_id) { constraints.push_back(constraint); } } parent_->data_.constraints = std::move(constraints); } + // Remove all 'data_.constraints' related to 'nodes_to_remove'. // If the removal lets other submaps lose all their inter-submap constraints, // delete their corresponding constraint submap matchers to save memory. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index e40da19d74..8a91e592ec 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1190,33 +1190,36 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished); - // Compile all nodes that are still INTRA_SUBMAP constrained once the submap - // with 'submap_id' is gone. + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. std::set nodes_to_retain; - for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - constraint.submap_id != submap_id) { - nodes_to_retain.insert(constraint.node_id); + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); } } - // Remove all 'data_.constraints' related to 'submap_id'. + + // Remove all nodes that are exlusively associated to 'submap_id'. std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. { std::vector constraints; for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.submap_id == submap_id) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - nodes_to_retain.count(constraint.node_id) == 0) { - // This node will no longer be INTRA_SUBMAP contrained and has to be - // removed. - nodes_to_remove.insert(constraint.node_id); - } - } else { + if (constraint.submap_id != submap_id) { constraints.push_back(constraint); } } parent_->data_.constraints = std::move(constraints); } + // Remove all 'data_.constraints' related to 'nodes_to_remove'. // If the removal lets other submaps lose all their inter-submap constraints, // delete their corresponding constraint submap matchers to save memory. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index e40433b31d..0ecc220970 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -323,6 +323,59 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) { } } +TEST_F(PoseGraph3DTest, EvenSubmapTrimmerOnFrozenTrajectory) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_keep = 10; + const int num_submaps_to_create = 2 * num_submaps_to_keep; + const int num_nodes_per_submap = 3; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = 42 + i; + auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * i + j; + auto node = testing::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); + // This step is normally done by a MapBuilder when loading frozen state. + pose_graph_->AddNodeToSubmap(NodeId{trajectory_id, node_index}, + SubmapId{trajectory_id, submap_index}); + } + } + pose_graph_->FreezeTrajectory(trajectory_id); + pose_graph_->AddTrimmer(absl::make_unique(trajectory_id)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + } +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 5621dbc759..ba8a8b7565 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -359,7 +359,7 @@ std::map MapBuilder::LoadState( if (load_frozen_state) { // Add information about which nodes belong to which submap. - // Required for 3D pure localization. + // This is required, even without constraints. for (const proto::PoseGraph::Constraint& constraint_proto : pose_graph_proto.constraint()) { if (constraint_proto.tag() != From 018dded2f9a0adb7e77003d05662ada251b541f5 Mon Sep 17 00:00:00 2001 From: Mac Mason Date: Wed, 20 Jan 2021 01:37:10 -0800 Subject: [PATCH 83/99] CHECK that ThreadPool is given a positive num_threads (#1800) Signed-off-by: Mac Mason --- cartographer/common/thread_pool.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/cartographer/common/thread_pool.cc b/cartographer/common/thread_pool.cc index c6295f895c..2457152980 100644 --- a/cartographer/common/thread_pool.cc +++ b/cartographer/common/thread_pool.cc @@ -37,6 +37,7 @@ void ThreadPoolInterface::SetThreadPool(Task* task) { } ThreadPool::ThreadPool(int num_threads) { + CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!"; absl::MutexLock locker(&mutex_); for (int i = 0; i != num_threads; ++i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); From a48f12dfa3eb6ea53a3fed637df555ed7c151f8f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 26 Jan 2021 18:26:46 +0100 Subject: [PATCH 84/99] Remove CHECK preventing the use of the IMU-based extrapolator. (#1804) Fixes #1803. Signed-off-by: Wolfgang Hess --- cartographer/mapping/pose_extrapolator_interface.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index b0b504f1b6..8030114989 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -84,8 +84,6 @@ PoseExtrapolatorInterface::CreateWithImuData( const std::vector& imu_data, const std::vector& initial_poses) { CHECK(!imu_data.empty()); - // TODO(schwoere): Implement/integrate imu based extrapolator. - CHECK(!options.use_imu_based()) << "Not implemented!"; if (options.use_imu_based()) { return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(), imu_data, initial_poses); From fbcfa65230a377a24cabff692bb19feabb3edbc6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 27 Jan 2021 13:50:22 +0100 Subject: [PATCH 85/99] Clean up the public headers in `common`. (#1805) The header ceres_solver_options.h is not used as part of public API anymore. lua_parameter_dictionary_test_helpers.h is only used in tests. Signed-off-by: Wolfgang Hess --- cartographer/common/{ => internal}/ceres_solver_options.cc | 2 +- cartographer/common/{ => internal}/ceres_solver_options.h | 0 .../testing}/lua_parameter_dictionary_test_helpers.h | 0 cartographer/common/lua_parameter_dictionary_test.cc | 2 +- cartographer/io/probability_grid_points_processor_test.cc | 2 +- cartographer/mapping/2d/range_data_inserter_2d_test.cc | 2 +- cartographer/mapping/2d/submap_2d_test.cc | 2 +- cartographer/mapping/3d/range_data_inserter_3d_test.cc | 2 +- cartographer/mapping/internal/2d/normal_estimation_2d_test.cc | 2 +- cartographer/mapping/internal/2d/pose_graph_2d_test.cc | 2 +- .../mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc | 2 +- .../internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc | 2 +- .../2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc | 2 +- .../scan_matching/real_time_correlative_scan_matcher_2d_test.cc | 2 +- .../2d/scan_matching/tsdf_match_cost_function_2d_test.cc | 2 +- .../mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc | 2 +- .../mapping/internal/3d/local_trajectory_builder_3d_test.cc | 2 +- .../mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc | 2 +- .../internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc | 2 +- .../3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc | 2 +- .../scan_matching/real_time_correlative_scan_matcher_3d_test.cc | 2 +- cartographer/mapping/internal/imu_based_pose_extrapolator.h | 2 +- cartographer/mapping/internal/motion_filter_test.cc | 2 +- .../mapping/internal/optimization/optimization_problem_2d.cc | 2 +- .../mapping/internal/optimization/optimization_problem_3d.cc | 2 +- .../internal/optimization/optimization_problem_3d_test.cc | 2 +- .../internal/optimization/optimization_problem_options.cc | 2 +- cartographer/mapping/internal/testing/test_helpers.cc | 2 +- cartographer/mapping/pose_extrapolator_interface.cc | 2 +- 29 files changed, 27 insertions(+), 27 deletions(-) rename cartographer/common/{ => internal}/ceres_solver_options.cc (96%) rename cartographer/common/{ => internal}/ceres_solver_options.h (100%) rename cartographer/common/{ => internal/testing}/lua_parameter_dictionary_test_helpers.h (100%) diff --git a/cartographer/common/ceres_solver_options.cc b/cartographer/common/internal/ceres_solver_options.cc similarity index 96% rename from cartographer/common/ceres_solver_options.cc rename to cartographer/common/internal/ceres_solver_options.cc index 25cd55815a..fd13e02ac9 100644 --- a/cartographer/common/ceres_solver_options.cc +++ b/cartographer/common/internal/ceres_solver_options.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" namespace cartographer { namespace common { diff --git a/cartographer/common/ceres_solver_options.h b/cartographer/common/internal/ceres_solver_options.h similarity index 100% rename from cartographer/common/ceres_solver_options.h rename to cartographer/common/internal/ceres_solver_options.h diff --git a/cartographer/common/lua_parameter_dictionary_test_helpers.h b/cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h similarity index 100% rename from cartographer/common/lua_parameter_dictionary_test_helpers.h rename to cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h diff --git a/cartographer/common/lua_parameter_dictionary_test.cc b/cartographer/common/lua_parameter_dictionary_test.cc index d794e0200a..ada3b7b363 100644 --- a/cartographer/common/lua_parameter_dictionary_test.cc +++ b/cartographer/common/lua_parameter_dictionary_test.cc @@ -21,7 +21,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index 8c48f6d697..1af0a25b94 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -18,8 +18,8 @@ #include +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" #include "cartographer/io/fake_file_writer.h" #include "cartographer/io/points_processor_pipeline_builder.h" diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index 1725334163..6b182aa57f 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -17,8 +17,8 @@ #include #include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/probability_values.h" diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 6b32b8abc4..d9b6e21d75 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -21,8 +21,8 @@ #include #include +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index 83dcd4eaeb..a7f92b31c7 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -19,7 +19,7 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc index bb6c453d4d..336d31dde6 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/internal/2d/normal_estimation_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/math.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 924f36e78c..2ecdfd12fe 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -21,7 +21,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 3590d31d8d..2fe2219896 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -20,7 +20,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc index 88ec321c7d..74036adfde 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -19,8 +19,8 @@ #include #include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index c50252f0ca..2de83935f5 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -22,7 +22,7 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 27df5a758b..b69cdd057a 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -21,7 +21,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/2d/tsdf_2d.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index 04c6f05aec..dcebdc7b19 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/tsdf_2d.h" #include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc index da06a893b3..0850e6ecf7 100644 --- a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 260264985c..e8f12691b6 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -20,7 +20,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc index 2c107532ba..5aab1fd327 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -21,7 +21,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" #include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc index 68a67703e0..bc0489fbbf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 0bcfecef89..df610e3978 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -22,7 +22,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc index 1d841a9047..da3223d9ea 100644 --- a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/imu_based_pose_extrapolator.h b/cartographer/mapping/internal/imu_based_pose_extrapolator.h index 1234fd00bd..6adc9b78e4 100644 --- a/cartographer/mapping/internal/imu_based_pose_extrapolator.h +++ b/cartographer/mapping/internal/imu_based_pose_extrapolator.h @@ -21,7 +21,7 @@ #include #include -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/imu_data.h" diff --git a/cartographer/mapping/internal/motion_filter_test.cc b/cartographer/mapping/internal/motion_filter_test.cc index 7ecc2d8a01..c5002fafe4 100644 --- a/cartographer/mapping/internal/motion_filter_test.cc +++ b/cartographer/mapping/internal/motion_filter_test.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/internal/motion_filter.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index c61d7965e9..3faf2556f4 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -24,7 +24,7 @@ #include #include -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h" diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 4494045453..fca36d1cdd 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -27,7 +27,7 @@ #include "Eigen/Core" #include "absl/memory/memory.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/3d/imu_integration.h" diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 92abbc6aca..26f9685a42 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/optimization/optimization_problem_options.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc index a17088ada9..39a0a561fe 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_options.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_options.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/internal/optimization/optimization_problem_options.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index f8401dd4f5..53fcd3dc4c 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/cartographer/mapping/internal/testing/test_helpers.cc @@ -19,7 +19,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/config.h" #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/sensor/timed_point_cloud_data.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index 8030114989..e408d3a018 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/pose_extrapolator_interface.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h" From 3abea342478d19ebae85427d1f60069cdd7e0e00 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 15 Feb 2021 17:07:49 +0100 Subject: [PATCH 86/99] Clean up the public headers in `mapping`. (#1806) The moved headers are not used in any public API. Signed-off-by: Wolfgang Hess --- cartographer/cloud/internal/client/trajectory_builder_stub.cc | 2 +- cartographer/cloud/internal/client/trajectory_builder_stub.h | 2 +- cartographer/cloud/internal/client_server_test.cc | 2 +- .../cloud/internal/handlers/add_sensor_data_batch_handler.cc | 2 +- cartographer/cloud/internal/map_builder_server.h | 2 +- cartographer/cloud/internal/sensor/serialization.h | 2 +- .../cloud/internal/testing/mock_map_builder_context.h | 2 +- cartographer/mapping/internal/2d/local_slam_result_2d.h | 2 +- .../mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h | 4 ++-- .../2d/scan_matching/real_time_correlative_scan_matcher_2d.cc | 2 +- .../real_time_correlative_scan_matcher_2d_test.cc | 2 +- .../internal/2d/scan_matching/tsdf_match_cost_function_2d.h | 2 +- .../2d/scan_matching/tsdf_match_cost_function_2d_test.cc | 2 +- cartographer/mapping/{ => internal}/2d/tsd_value_converter.cc | 2 +- cartographer/mapping/{ => internal}/2d/tsd_value_converter.h | 0 .../mapping/{ => internal}/2d/tsd_value_converter_test.cc | 2 +- cartographer/mapping/{ => internal}/2d/tsdf_2d.cc | 2 +- cartographer/mapping/{ => internal}/2d/tsdf_2d.h | 3 ++- cartographer/mapping/{ => internal}/2d/tsdf_2d_test.cc | 2 +- .../mapping/internal/2d/tsdf_range_data_inserter_2d.h | 2 +- cartographer/mapping/internal/3d/local_slam_result_3d.h | 2 +- cartographer/mapping/internal/collated_trajectory_builder.h | 2 +- cartographer/mapping/internal/global_trajectory_builder.cc | 2 +- cartographer/mapping/internal/global_trajectory_builder.h | 2 +- cartographer/mapping/{ => internal}/local_slam_result_data.h | 0 cartographer/mapping/internal/range_data_collator.cc | 2 +- cartographer/mapping/trajectory_builder_interface.cc | 2 +- 27 files changed, 27 insertions(+), 26 deletions(-) rename cartographer/mapping/{ => internal}/2d/tsd_value_converter.cc (95%) rename cartographer/mapping/{ => internal}/2d/tsd_value_converter.h (100%) rename cartographer/mapping/{ => internal}/2d/tsd_value_converter_test.cc (98%) rename cartographer/mapping/{ => internal}/2d/tsdf_2d.cc (99%) rename cartographer/mapping/{ => internal}/2d/tsdf_2d.h (97%) rename cartographer/mapping/{ => internal}/2d/tsdf_2d_test.cc (99%) rename cartographer/mapping/{ => internal}/local_slam_result_data.h (100%) diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.cc b/cartographer/cloud/internal/client/trajectory_builder_stub.cc index 104d92c910..b12caa86b8 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.cc +++ b/cartographer/cloud/internal/client/trajectory_builder_stub.cc @@ -18,7 +18,7 @@ #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.h b/cartographer/cloud/internal/client/trajectory_builder_stub.h index 95251a9147..c57a4f3a9a 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.h +++ b/cartographer/cloud/internal/client/trajectory_builder_stub.h @@ -26,7 +26,7 @@ #include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" #include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" #include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "grpc++/grpc++.h" #include "pose_graph_stub.h" diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 1eaea52e15..ade3c98fb7 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -24,11 +24,11 @@ #include "cartographer/io/internal/testing/test_helpers.h" #include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream_deserializer.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/testing/mock_map_builder.h" #include "cartographer/mapping/internal/testing/mock_pose_graph.h" #include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" #include "cartographer/mapping/internal/testing/test_helpers.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/map_builder.h" #include "glog/logging.h" #include "gmock/gmock.h" diff --git a/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc b/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc index cb315b1b42..c277428bcf 100644 --- a/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc +++ b/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc @@ -20,7 +20,7 @@ #include "async_grpc/rpc_handler.h" #include "cartographer/cloud/internal/map_builder_context_interface.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/metrics/counter.h" #include "cartographer/sensor/internal/dispatchable.h" #include "cartographer/sensor/timed_point_cloud_data.h" diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index ed7594362b..03a70aa405 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/cartographer/cloud/internal/map_builder_server.h @@ -27,8 +27,8 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/internal/dispatchable.h" diff --git a/cartographer/cloud/internal/sensor/serialization.h b/cartographer/cloud/internal/sensor/serialization.h index 46ee12686e..9da7fe2cea 100644 --- a/cartographer/cloud/internal/sensor/serialization.h +++ b/cartographer/cloud/internal/sensor/serialization.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_CLOUD_INTERNAL_SENSOR_SERIALIZATION_H #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" diff --git a/cartographer/cloud/internal/testing/mock_map_builder_context.h b/cartographer/cloud/internal/testing/mock_map_builder_context.h index 9ec8788ed5..bb0e1d7abe 100644 --- a/cartographer/cloud/internal/testing/mock_map_builder_context.h +++ b/cartographer/cloud/internal/testing/mock_map_builder_context.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_MAP_BUILDER_CONTEXT_H #include "cartographer/cloud/internal/map_builder_context_interface.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/2d/local_slam_result_2d.h b/cartographer/mapping/internal/2d/local_slam_result_2d.h index f55fdd25bb..71b4f181ef 100644 --- a/cartographer/mapping/internal/2d/local_slam_result_2d.h +++ b/cartographer/mapping/internal/2d/local_slam_result_2d.h @@ -17,8 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h index 1784d07c8b..5b014fd45c 100644 --- a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h @@ -19,7 +19,7 @@ #include -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" namespace cartographer { namespace mapping { @@ -133,4 +133,4 @@ class InterpolatedTSDF2D { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ \ No newline at end of file +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 1bc75bd056..60b00fe4c3 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -25,7 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index b69cdd057a..71fb0a9604 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h index d66ef48668..8a1ce6dae2 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index dcebdc7b19..8c9cf04813 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -18,7 +18,7 @@ #include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/2d/tsd_value_converter.cc b/cartographer/mapping/internal/2d/tsd_value_converter.cc similarity index 95% rename from cartographer/mapping/2d/tsd_value_converter.cc rename to cartographer/mapping/internal/2d/tsd_value_converter.cc index dde8ddb4a7..5feddad786 100644 --- a/cartographer/mapping/2d/tsd_value_converter.cc +++ b/cartographer/mapping/internal/2d/tsd_value_converter.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsd_value_converter.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/2d/tsd_value_converter.h b/cartographer/mapping/internal/2d/tsd_value_converter.h similarity index 100% rename from cartographer/mapping/2d/tsd_value_converter.h rename to cartographer/mapping/internal/2d/tsd_value_converter.h diff --git a/cartographer/mapping/2d/tsd_value_converter_test.cc b/cartographer/mapping/internal/2d/tsd_value_converter_test.cc similarity index 98% rename from cartographer/mapping/2d/tsd_value_converter_test.cc rename to cartographer/mapping/internal/2d/tsd_value_converter_test.cc index 0c37d2dd53..1fa496e040 100644 --- a/cartographer/mapping/2d/tsd_value_converter_test.cc +++ b/cartographer/mapping/internal/2d/tsd_value_converter_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsd_value_converter.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/internal/2d/tsdf_2d.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_2d.cc rename to cartographer/mapping/internal/2d/tsdf_2d.cc index 32bdb59632..fa6fc9b756 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/internal/2d/tsdf_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "absl/memory/memory.h" diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/internal/2d/tsdf_2d.h similarity index 97% rename from cartographer/mapping/2d/tsdf_2d.h rename to cartographer/mapping/internal/2d/tsdf_2d.h index 250f38f08e..16134f637e 100644 --- a/cartographer/mapping/2d/tsdf_2d.h +++ b/cartographer/mapping/internal/2d/tsdf_2d.h @@ -22,8 +22,9 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/2d/tsd_value_converter.h" #include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" + namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_2d_test.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_2d_test.cc rename to cartographer/mapping/internal/2d/tsdf_2d_test.cc index 5e50343ac1..2ea899ec26 100644 --- a/cartographer/mapping/2d/tsdf_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include diff --git a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h index a3fa5ba775..bdc810ac5c 100644 --- a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/3d/local_slam_result_3d.h b/cartographer/mapping/internal/3d/local_slam_result_3d.h index 3efba2ed2e..88e94ffa57 100644 --- a/cartographer/mapping/internal/3d/local_slam_result_3d.h +++ b/cartographer/mapping/internal/3d/local_slam_result_3d.h @@ -17,8 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" namespace cartographer { diff --git a/cartographer/mapping/internal/collated_trajectory_builder.h b/cartographer/mapping/internal/collated_trajectory_builder.h index e9e39ffc01..c00a979ad7 100644 --- a/cartographer/mapping/internal/collated_trajectory_builder.h +++ b/cartographer/mapping/internal/collated_trajectory_builder.h @@ -25,7 +25,7 @@ #include "cartographer/common/internal/rate_timer.h" #include "cartographer/common/port.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/collator_interface.h" diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index f68645f14a..a60e88a742 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -21,8 +21,8 @@ #include "absl/memory/memory.h" #include "absl/types/optional.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/motion_filter.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/metrics/family_factory.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/global_trajectory_builder.h b/cartographer/mapping/internal/global_trajectory_builder.h index 46694367a2..c0980f5848 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.h +++ b/cartographer/mapping/internal/global_trajectory_builder.h @@ -23,7 +23,7 @@ #include "cartographer/mapping/internal/2d/pose_graph_2d.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" #include "cartographer/mapping/internal/3d/pose_graph_3d.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/metrics/family_factory.h" diff --git a/cartographer/mapping/local_slam_result_data.h b/cartographer/mapping/internal/local_slam_result_data.h similarity index 100% rename from cartographer/mapping/local_slam_result_data.h rename to cartographer/mapping/internal/local_slam_result_data.h diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 6c27ac3d67..3ce9e2145d 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -19,7 +19,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "glog/logging.h" namespace cartographer { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 991dcc4dca..b81eed5bce 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -18,7 +18,7 @@ #include "cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" namespace cartographer { namespace mapping { From 105c034577220268cd28a304a185adbec46b729f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 11 Mar 2021 12:05:30 +0100 Subject: [PATCH 87/99] Prepare 2.0.0 release. (#1819) Towards #1716. Signed-off-by: Wolfgang Hess --- CHANGELOG.rst | 6 +++++- package.xml | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0e47fa611a..05e6da5f6e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,12 @@ Changelog for package cartographer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.0.0 (2021-03-09) +------------------ +https://github.com/cartographer-project/cartographer/compare/1.0.0...2.0.0 + 1.0.0 (2018-06-01) ----------------------- +------------------ https://github.com/googlecartographer/cartographer/compare/0.3.0...1.0.0 0.3.0 (2017-11-23) diff --git a/package.xml b/package.xml index 2d4d317b6f..8c9f1204ec 100644 --- a/package.xml +++ b/package.xml @@ -17,7 +17,7 @@ cartographer - 1.0.0 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor From b8228ee6564f5a7ad0d6d0b9a30516521cff2ee9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 7 May 2021 15:52:57 +0200 Subject: [PATCH 88/99] Remove Ubuntu Xenial from CI. (#1833) Ubuntu 16.04 has reached end of standard support. Signed-off-by: Wolfgang Hess --- .travis.yml | 1 - Dockerfile.xenial | 36 ------------------------------------ cmake/functions.cmake | 4 ---- docs/source/index.rst | 6 +++--- 4 files changed, 3 insertions(+), 44 deletions(-) delete mode 100644 Dockerfile.xenial diff --git a/.travis.yml b/.travis.yml index 987d35c762..8dc103144e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,7 +22,6 @@ cache: - /home/travis/docker/ env: - - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ diff --git a/Dockerfile.xenial b/Dockerfile.xenial deleted file mode 100644 index 83620f90cf..0000000000 --- a/Dockerfile.xenial +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright 2016 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. - -FROM ubuntu:xenial - -ARG cc -ARG cxx - -# Set the preferred C/C++ compiler toolchain, if given (otherwise default). -ENV CC=$cc -ENV CXX=$cxx - -# This base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* - -COPY scripts/install_debs_cmake.sh cartographer/scripts/ -RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_abseil.sh cartographer/scripts/ -RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf -COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/cmake/functions.cmake b/cmake/functions.cmake index 1bbce430c2..b3f57e829d 100644 --- a/cmake/functions.cmake +++ b/cmake/functions.cmake @@ -78,10 +78,6 @@ macro(google_initialize_cartographer_project) else() set(GOOG_CXX_FLAGS "-pthread -fPIC ${GOOG_CXX_FLAGS}") - if (CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 6.1) - google_add_flag(GOOG_CXX_FLAGS "-std=c++11") - endif() - google_add_flag(GOOG_CXX_FLAGS "-Wall") google_add_flag(GOOG_CXX_FLAGS "-Wpedantic") diff --git a/docs/source/index.rst b/docs/source/index.rst index 8de158acbb..4871233065 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -65,7 +65,7 @@ Getting started without ROS Please see our ROS integration as a starting point for integrating your system with the standalone library. Currently, it is the best available reference. -On Ubuntu 16.04 (Xenial): +On Ubuntu 18.04 (Bionic): .. literalinclude:: ../../scripts/install_debs_cmake.sh :language: bash @@ -97,8 +97,8 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 16.04 (Xenial), 18.04 (Bionic), 20.04 (Focal) -* gcc version 4.8.4, 5.4.0, 7.5.0, 9.3.0 +* Ubuntu 18.04 (Bionic), 20.04 (Focal) +* gcc version 6.3.0, 7.5.0, 9.3.0 Known Issues ------------ From f440cc21cb8f4a3568893d57956c7e6f50279486 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 29 Jun 2022 10:39:10 +0200 Subject: [PATCH 89/99] Move to GitHub Actions for CI. (#1884) This moves away from travis-ci.org which no longer works. Signed-off-by: Wolfgang Hess --- .dockerignore | 1 - .github/workflows/ci-bionic.yaml | 15 +++++++++++++ .github/workflows/ci-buster.yaml | 15 +++++++++++++ .github/workflows/ci-focal.yaml | 15 +++++++++++++ .travis.yml | 36 -------------------------------- README.rst | 6 +++--- scripts/load_docker_cache.sh | 26 ----------------------- scripts/save_docker_cache.sh | 30 -------------------------- 8 files changed, 48 insertions(+), 96 deletions(-) create mode 100644 .github/workflows/ci-bionic.yaml create mode 100644 .github/workflows/ci-buster.yaml create mode 100644 .github/workflows/ci-focal.yaml delete mode 100644 .travis.yml delete mode 100755 scripts/load_docker_cache.sh delete mode 100755 scripts/save_docker_cache.sh diff --git a/.dockerignore b/.dockerignore index 6647fc9fac..39ba3cccd9 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,4 +1,3 @@ **/Dockerfile* **/.dockerignore **/.git -**/.travis.yml diff --git a/.github/workflows/ci-bionic.yaml b/.github/workflows/ci-bionic.yaml new file mode 100644 index 0000000000..76b6c90f52 --- /dev/null +++ b/.github/workflows/ci-bionic.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 18.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.bionic diff --git a/.github/workflows/ci-buster.yaml b/.github/workflows/ci-buster.yaml new file mode 100644 index 0000000000..0faf26eddc --- /dev/null +++ b/.github/workflows/ci-buster.yaml @@ -0,0 +1,15 @@ +name: Debian Buster CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.buster diff --git a/.github/workflows/ci-focal.yaml b/.github/workflows/ci-focal.yaml new file mode 100644 index 0000000000..e1bc5cbf4a --- /dev/null +++ b/.github/workflows/ci-focal.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 20.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.focal diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 8dc103144e..0000000000 --- a/.travis.yml +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright 2016 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. - -sudo: required -services: docker - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html -cache: - directories: - - /home/travis/docker/ - -env: - - LSB_RELEASE=bionic DOCKER_CACHE_FILE=/home/travis/docker/bionic-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=focal DOCKER_CACHE_FILE=/home/travis/docker/focal-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=buster DOCKER_CACHE_FILE=/home/travis/docker/buster-cache.tar.gz CC=gcc CXX=g++ - -before_install: scripts/load_docker_cache.sh - -install: true -script: - - docker build ${TRAVIS_BUILD_DIR} -t cartographer:${LSB_RELEASE} -f Dockerfile.${LSB_RELEASE} - --build-arg cc=$CC --build-arg cxx=$CXX - - scripts/save_docker_cache.sh diff --git a/README.rst b/README.rst index 1b216cbc17..9995a49cf0 100644 --- a/README.rst +++ b/README.rst @@ -83,10 +83,10 @@ Slides of these Cartographer Open House meetings are listed below. - June 22, 2017: `Slides `_ - June 8, 2017: `Slides `_ -.. |build| image:: https://travis-ci.org/cartographer-project/cartographer.svg?branch=master - :alt: Build Status +.. |build| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml/badge.svg + :alt: Ubuntu 20.04 Build Status :scale: 100% - :target: https://travis-ci.org/cartographer-project/cartographer + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest :alt: Documentation Status :scale: 100% diff --git a/scripts/load_docker_cache.sh b/scripts/load_docker_cache.sh deleted file mode 100755 index 4c37247b2b..0000000000 --- a/scripts/load_docker_cache.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/bash - -# Copyright 2016 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. - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html - -set -o errexit -set -o verbose -set -o pipefail - -if [ -f ${DOCKER_CACHE_FILE} ]; then - gunzip -c ${DOCKER_CACHE_FILE} | docker load; -fi diff --git a/scripts/save_docker_cache.sh b/scripts/save_docker_cache.sh deleted file mode 100755 index 7b2000e576..0000000000 --- a/scripts/save_docker_cache.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/bash - -# Copyright 2016 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. - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html - -set -o errexit -set -o verbose -set -o pipefail - -if [[ ${TRAVIS_BRANCH} == "master" ]] && - [[ ${TRAVIS_PULL_REQUEST} == "false" ]]; then - mkdir -p $(dirname ${DOCKER_CACHE_FILE}) - IMAGE_NAMES=$(docker history -q cartographer:${LSB_RELEASE} | grep -v '') - docker save ${IMAGE_NAMES} | gzip > ${DOCKER_CACHE_FILE}.new - mv ${DOCKER_CACHE_FILE}.new ${DOCKER_CACHE_FILE} -fi From b1b63a3cb1e24098b1473e79cc05be7116e2a48a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 29 Jun 2022 05:19:08 -0400 Subject: [PATCH 90/99] Add libabsl-dev to the package.xml dependencies. (#1875) This will allow the ROS buildfarm to properly install dependencies when attempting to build this package. Signed-off-by: Chris Lalancette --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 8c9f1204ec..cabc5576b9 100644 --- a/package.xml +++ b/package.xml @@ -43,6 +43,7 @@ libboost-iostreams-dev eigen + libabsl-dev libcairo2-dev libceres-dev libgflags-dev From f76f849e6a9912f18493818728e170de57cc366d Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 4 Jul 2022 19:38:24 +0200 Subject: [PATCH 91/99] Add Ubuntu 22.04 to the install scripts, CI, docs. (#1888) Signed-off-by: Wolfgang Hess --- .github/workflows/ci-jammy.yaml | 15 +++++++++++++++ Dockerfile.bionic | 2 +- Dockerfile.focal | 2 +- Dockerfile.jammy | 31 +++++++++++++++++++++++++++++++ README.rst | 8 ++++++-- docs/source/index.rst | 8 ++------ scripts/install_abseil.sh | 2 +- scripts/install_debs_cmake.sh | 24 +++++++++++------------- 8 files changed, 68 insertions(+), 24 deletions(-) create mode 100644 .github/workflows/ci-jammy.yaml create mode 100644 Dockerfile.jammy diff --git a/.github/workflows/ci-jammy.yaml b/.github/workflows/ci-jammy.yaml new file mode 100644 index 0000000000..104ed9561a --- /dev/null +++ b/.github/workflows/ci-jammy.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 22.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.jammy diff --git a/Dockerfile.bionic b/Dockerfile.bionic index 7066e6aaca..d79a257be2 100644 --- a/Dockerfile.bionic +++ b/Dockerfile.bionic @@ -22,7 +22,7 @@ ENV CC=$cc ENV CXX=$cxx # This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later -# when it would wait for user input. +# when it would wait for user input. RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ diff --git a/Dockerfile.focal b/Dockerfile.focal index 4abbe56504..e7575241fb 100644 --- a/Dockerfile.focal +++ b/Dockerfile.focal @@ -22,7 +22,7 @@ ENV CC=$cc ENV CXX=$cxx # This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later -# when it would wait for user input. +# when it would wait for user input. RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ diff --git a/Dockerfile.jammy b/Dockerfile.jammy new file mode 100644 index 0000000000..7e5fb9bc8a --- /dev/null +++ b/Dockerfile.jammy @@ -0,0 +1,31 @@ +# Copyright 2020 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. + +FROM ubuntu:jammy + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/README.rst b/README.rst index 9995a49cf0..2b89e40b6b 100644 --- a/README.rst +++ b/README.rst @@ -16,7 +16,7 @@ Cartographer ============ -|build| |docs| |license| +|build-jammy| |build-focal| |docs| |license| Purpose ======= @@ -83,7 +83,11 @@ Slides of these Cartographer Open House meetings are listed below. - June 22, 2017: `Slides `_ - June 8, 2017: `Slides `_ -.. |build| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml/badge.svg +.. |build-jammy| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-jammy.yaml/badge.svg + :alt: Ubuntu 22.04 Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-jammy.yaml +.. |build-focal| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml/badge.svg :alt: Ubuntu 20.04 Build Status :scale: 100% :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml diff --git a/docs/source/index.rst b/docs/source/index.rst index 4871233065..29d5279f8b 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -75,10 +75,6 @@ On Ubuntu 18.04 (Bionic): :language: bash :lines: 20- -.. literalinclude:: ../../scripts/install_ceres.sh - :language: bash - :lines: 20- - .. literalinclude:: ../../scripts/install_proto3.sh :language: bash :lines: 20- @@ -97,8 +93,8 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 18.04 (Bionic), 20.04 (Focal) -* gcc version 6.3.0, 7.5.0, 9.3.0 +* Ubuntu 18.04 (Bionic), 20.04 (Focal), 22.04 (Jammy) +* gcc version 6.3.0, 7.5.0, 9.3.0, 11.2.0 Known Issues ------------ diff --git a/scripts/install_abseil.sh b/scripts/install_abseil.sh index 4962d6ecc9..ca5740b7e9 100755 --- a/scripts/install_abseil.sh +++ b/scripts/install_abseil.sh @@ -19,7 +19,7 @@ set -o verbose git clone https://github.com/abseil/abseil-cpp.git cd abseil-cpp -git checkout d902eb869bcfacc1bad14933ed9af4bed006d481 +git checkout 215105818dfde3174fe799600bb0f3cae233d0bf # 20211102.0 mkdir build cd build cmake -G Ninja \ diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index 7eda82a3ac..932210a781 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -37,17 +37,15 @@ sudo apt-get install -y \ ninja-build \ stow -# Install Ceres Solver and Protocol Buffers support if available. +# Install Ceres Solver, Protocol Buffers, Abseil support if available. # No need to build it ourselves. -if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] -then - sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler -else - sudo apt-get install -y python-sphinx - if [[ "$(lsb_release -sc)" = "bionic" ]] - then - sudo apt-get install -y libceres-dev - fi -fi - - +case "$(lsb_release -sc)" in + jammy) + sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler libabsl-dev ;; + focal|buster) + sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler ;; + bionic) + sudo apt-get install -y python-sphinx libceres-dev ;; + *) + sudo apt-get install -y python-sphinx ;; +esac From 5663e340c0c835df86c99d378f4770ab00744fb8 Mon Sep 17 00:00:00 2001 From: Takashi Ogura Date: Tue, 5 Jul 2022 02:39:13 +0900 Subject: [PATCH 92/99] Fix typo in trajectory_builder_3d.lua (#1870) Fix the comment (`use_intensites` -> `use_intensities`) Signed-off-by: Takashi Ogura --- configuration_files/trajectory_builder_3d.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 489889d867..b9208d1b27 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -106,7 +106,7 @@ TRAJECTORY_BUILDER_3D = { }, }, - -- When setting use_intensites to true, the intensity_cost_function_options_0 + -- When setting use_intensities to true, the intensity_cost_function_options_0 -- parameter in ceres_scan_matcher has to be set up as well or otherwise -- CeresScanMatcher will CHECK-fail. use_intensities = false, From 7b8b29d6d50d4562df1f970fa3125a443d6201dd Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Sat, 16 Jul 2022 13:05:24 +0200 Subject: [PATCH 93/99] Fix CI for Ubuntu 18.04 by disabling gRPC test build for now. (#1891) Signed-off-by: Wolfgang Hess --- Dockerfile.bionic | 8 +------- Dockerfile.bionic.grpc | 41 +++++++++++++++++++++++++++++++++++++++++ README.rst | 6 +++++- 3 files changed, 47 insertions(+), 8 deletions(-) create mode 100644 Dockerfile.bionic.grpc diff --git a/Dockerfile.bionic b/Dockerfile.bionic index d79a257be2..f6e44347ba 100644 --- a/Dockerfile.bionic +++ b/Dockerfile.bionic @@ -31,11 +31,5 @@ COPY scripts/install_abseil.sh cartographer/scripts/ RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_proto3.sh cartographer/scripts/ RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf -COPY scripts/install_grpc.sh cartographer/scripts/ -RUN cartographer/scripts/install_grpc.sh && rm -rf grpc -COPY scripts/install_async_grpc.sh cartographer/scripts/ -RUN cartographer/scripts/install_async_grpc.sh && rm -rf async_grpc -COPY scripts/install_prometheus_cpp.sh cartographer/scripts/ -RUN cartographer/scripts/install_prometheus_cpp.sh && rm -rf prometheus-cpp COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake_with_grpc.sh && rm -rf cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.bionic.grpc b/Dockerfile.bionic.grpc new file mode 100644 index 0000000000..d79a257be2 --- /dev/null +++ b/Dockerfile.bionic.grpc @@ -0,0 +1,41 @@ +# Copyright 2020 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. + +FROM ubuntu:bionic + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_proto3.sh cartographer/scripts/ +RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY scripts/install_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_grpc.sh && rm -rf grpc +COPY scripts/install_async_grpc.sh cartographer/scripts/ +RUN cartographer/scripts/install_async_grpc.sh && rm -rf async_grpc +COPY scripts/install_prometheus_cpp.sh cartographer/scripts/ +RUN cartographer/scripts/install_prometheus_cpp.sh && rm -rf prometheus-cpp +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake_with_grpc.sh && rm -rf cartographer diff --git a/README.rst b/README.rst index 2b89e40b6b..d66c52cd3f 100644 --- a/README.rst +++ b/README.rst @@ -16,7 +16,7 @@ Cartographer ============ -|build-jammy| |build-focal| |docs| |license| +|build-jammy| |build-focal| |build-bionic| |docs| |license| Purpose ======= @@ -91,6 +91,10 @@ Slides of these Cartographer Open House meetings are listed below. :alt: Ubuntu 20.04 Build Status :scale: 100% :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml +.. |build-bionic| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bionic.yaml/badge.svg + :alt: Ubuntu 18.04 Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bionic.yaml .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest :alt: Documentation Status :scale: 100% From dc6263414b447d3dbf20d4553567f687d9be5b10 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 19 Jul 2022 20:26:53 +0200 Subject: [PATCH 94/99] Remove Debian Stretch from CI. (#1895) It has reached end-of-life with the end of LTS on June 30, 2022. Signed-off-by: Wolfgang Hess --- Dockerfile.stretch | 36 ----------------------------------- docs/source/index.rst | 2 +- scripts/install_ceres.sh | 31 ------------------------------ scripts/install_debs_cmake.sh | 12 ++++++------ 4 files changed, 7 insertions(+), 74 deletions(-) delete mode 100644 Dockerfile.stretch delete mode 100755 scripts/install_ceres.sh diff --git a/Dockerfile.stretch b/Dockerfile.stretch deleted file mode 100644 index b3f346e78d..0000000000 --- a/Dockerfile.stretch +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright 2016 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. - -FROM debian:stretch - -ARG cc -ARG cxx - -# Set the preferred C/C++ compiler toolchain, if given (otherwise default). -ENV CC=$cc -ENV CXX=$cxx - -# This base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* - -COPY scripts/install_debs_cmake.sh cartographer/scripts/ -RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_abseil.sh cartographer/scripts/ -RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf -COPY . cartographer -RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/docs/source/index.rst b/docs/source/index.rst index 29d5279f8b..6273f0491f 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -94,7 +94,7 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM * Ubuntu 18.04 (Bionic), 20.04 (Focal), 22.04 (Jammy) -* gcc version 6.3.0, 7.5.0, 9.3.0, 11.2.0 +* gcc version 7.5.0, 9.3.0, 11.2.0 Known Issues ------------ diff --git a/scripts/install_ceres.sh b/scripts/install_ceres.sh deleted file mode 100755 index 56999d09c4..0000000000 --- a/scripts/install_ceres.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/sh - -# Copyright 2016 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. - -set -o errexit -set -o verbose - -VERSION="1.13.0" - -# Build and install Ceres. -git clone https://ceres-solver.googlesource.com/ceres-solver -cd ceres-solver -git checkout tags/${VERSION} -mkdir build -cd build -cmake .. -G Ninja -DCXX11=ON -ninja -CTEST_OUTPUT_ON_FAILURE=1 ninja test -sudo ninja install diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index 932210a781..8c802fc4d9 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -27,6 +27,7 @@ sudo apt-get install -y \ google-mock \ libboost-all-dev \ libcairo2-dev \ + libceres-dev \ libcurl4-openssl-dev \ libeigen3-dev \ libgflags-dev \ @@ -35,17 +36,16 @@ sudo apt-get install -y \ libsuitesparse-dev \ lsb-release \ ninja-build \ + python3-sphinx \ stow -# Install Ceres Solver, Protocol Buffers, Abseil support if available. +# Install Protocol Buffers and Abseil if available. # No need to build it ourselves. case "$(lsb_release -sc)" in jammy) - sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler libabsl-dev ;; + sudo apt-get install -y libgmock-dev protobuf-compiler libabsl-dev ;; focal|buster) - sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler ;; + sudo apt-get install -y libgmock-dev protobuf-compiler ;; bionic) - sudo apt-get install -y python-sphinx libceres-dev ;; - *) - sudo apt-get install -y python-sphinx ;; + ;; esac From ca526de1f84cf85d5ea6345deea0bb4104f8c715 Mon Sep 17 00:00:00 2001 From: Linh Nguyen Date: Fri, 22 Jul 2022 01:47:18 +0800 Subject: [PATCH 95/99] Fix crash caused by setting gravity lower bound (#1893) Signed-off-by: Linh Nguyen --- .../internal/optimization/optimization_problem_3d.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index fca36d1cdd..246707b26d 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -374,6 +374,8 @@ void OptimizationProblem3D::Solve( auto imu_it = imu_data.begin(); auto prev_node_it = node_it; + + bool gravity_block_added = false; for (++node_it; node_it != trajectory_end; ++node_it) { const NodeId first_node_id = prev_node_it->id; const NodeSpec3D& first_node_data = prev_node_it->data; @@ -432,6 +434,7 @@ void OptimizationProblem3D::Solve( C_nodes.at(third_node_id).translation(), &trajectory_data.gravity_constant, trajectory_data.imu_calibration.data()); + gravity_block_added = true; } problem.AddResidualBlock( RotationCostFunction3D::CreateAutoDiffCostFunction( @@ -442,8 +445,11 @@ void OptimizationProblem3D::Solve( trajectory_data.imu_calibration.data()); } - // Force gravity constant to be positive. - problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, 0.0); + if (gravity_block_added) { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, + 0.0); + } } } From 1173e80c45e77676714141bfda35eca49b7826db Mon Sep 17 00:00:00 2001 From: XiaotaoGuo <45047478+XiaotaoGuo@users.noreply.github.com> Date: Sat, 23 Jul 2022 21:10:49 +0800 Subject: [PATCH 96/99] removed unused param from cmake macro (#1847) Signed-off-by: XiaotaoGuo --- cmake/functions.cmake | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/functions.cmake b/cmake/functions.cmake index b3f57e829d..8cfc05ff97 100644 --- a/cmake/functions.cmake +++ b/cmake/functions.cmake @@ -22,7 +22,7 @@ macro(_parse_arguments ARGS) "${OPTIONS}" "${ONE_VALUE_ARG}" "${MULTI_VALUE_ARGS}" ${ARGS}) endmacro(_parse_arguments) -macro(_common_compile_stuff VISIBILITY) +macro(_common_compile_stuff) set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${NAME} PROPERTIES @@ -34,7 +34,7 @@ endmacro(_common_compile_stuff) function(google_test NAME ARG_SRC) add_executable(${NAME} ${ARG_SRC}) - _common_compile_stuff("PRIVATE") + _common_compile_stuff() # Make sure that gmock always includes the correct gtest/gtest.h. target_include_directories("${NAME}" SYSTEM PRIVATE @@ -49,7 +49,7 @@ function(google_binary NAME) add_executable(${NAME} ${ARG_SRCS}) - _common_compile_stuff("PRIVATE") + _common_compile_stuff() install(TARGETS "${NAME}" RUNTIME DESTINATION bin) endfunction() From 9ab557476d3d63e652f928776e6723a8912b948a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 28 Jul 2022 08:09:21 +0200 Subject: [PATCH 97/99] Add Debian Bullseye to the install scripts, CI, docs. (#1897) Signed-off-by: Wolfgang Hess --- .github/workflows/ci-bullseye.yaml | 15 +++++++++++++++ Dockerfile.bullseye | 30 ++++++++++++++++++++++++++++++ README.rst | 10 +++++++++- docs/source/index.rst | 2 +- scripts/install_debs_cmake.sh | 2 +- 5 files changed, 56 insertions(+), 3 deletions(-) create mode 100644 .github/workflows/ci-bullseye.yaml create mode 100644 Dockerfile.bullseye diff --git a/.github/workflows/ci-bullseye.yaml b/.github/workflows/ci-bullseye.yaml new file mode 100644 index 0000000000..2144453776 --- /dev/null +++ b/.github/workflows/ci-bullseye.yaml @@ -0,0 +1,15 @@ +name: Debian Bullseye CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.bullseye diff --git a/Dockerfile.bullseye b/Dockerfile.bullseye new file mode 100644 index 0000000000..4e9fd92ba1 --- /dev/null +++ b/Dockerfile.bullseye @@ -0,0 +1,30 @@ +# Copyright 2022 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. + +FROM debian:bullseye + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo. +RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/README.rst b/README.rst index d66c52cd3f..2bdf8aa000 100644 --- a/README.rst +++ b/README.rst @@ -16,7 +16,7 @@ Cartographer ============ -|build-jammy| |build-focal| |build-bionic| |docs| |license| +|build-jammy| |build-focal| |build-bionic| |build-bullseye| |build-buster| |docs| |license| Purpose ======= @@ -95,6 +95,14 @@ Slides of these Cartographer Open House meetings are listed below. :alt: Ubuntu 18.04 Build Status :scale: 100% :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bionic.yaml +.. |build-bullseye| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bullseye.yaml/badge.svg + :alt: Debian Bullseye Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bullseye.yaml +.. |build-buster| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-buster.yaml/badge.svg + :alt: Debian Buster Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-buster.yaml .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest :alt: Documentation Status :scale: 100% diff --git a/docs/source/index.rst b/docs/source/index.rst index 6273f0491f..6614f6fe0b 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -94,7 +94,7 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM * Ubuntu 18.04 (Bionic), 20.04 (Focal), 22.04 (Jammy) -* gcc version 7.5.0, 9.3.0, 11.2.0 +* gcc version 7.5.0, 8.3.0, 9.3.0, 10.2.1, 11.2.0 Known Issues ------------ diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index 8c802fc4d9..18752ec69d 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -42,7 +42,7 @@ sudo apt-get install -y \ # Install Protocol Buffers and Abseil if available. # No need to build it ourselves. case "$(lsb_release -sc)" in - jammy) + jammy|bullseye) sudo apt-get install -y libgmock-dev protobuf-compiler libabsl-dev ;; focal|buster) sudo apt-get install -y libgmock-dev protobuf-compiler ;; From ef00de2317dcf7895b09f18cc4d87f8b533a019b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?X=C3=B9d=C5=8Dng=20Y=C3=A1ng?= Date: Wed, 7 Sep 2022 05:44:43 +1000 Subject: [PATCH 98/99] update rules_boost to latest version (#1898) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Xùdōng Yáng --- bazel/repositories.bzl | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index c4bd67eb0f..3be7834950 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -20,11 +20,10 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_nelhage_rules_boost", - sha256 = "371f49e7b29e44a718baf8b9a2dd3eca865005a851c9ecf8fb6a10a715aa58dd", - strip_prefix = "rules_boost-a5a95642f6097f8949020646ffe89d7243008981", + sha256 = "f7d620c0061631d5b7685cd1065f2e2bf0768559555010a75e8e4720006f5867", + strip_prefix = "rules_boost-c3fae06e819ed8b93e31b150387dce4864758643", urls = [ - "https://mirror.bazel.build/github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", - "https://github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", + "https://github.com/nelhage/rules_boost/archive/c3fae06e819ed8b93e31b150387dce4864758643.tar.gz", ], ) From 877157a0d91788a7700221d87232d412cb3c1ef4 Mon Sep 17 00:00:00 2001 From: Katherine Scott Date: Fri, 5 Jan 2024 11:05:32 -0800 Subject: [PATCH 99/99] Add note for ROS users. (#1941) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * update rules_boost to latest version (#1898) Signed-off-by: Xùdōng Yáng Signed-off-by: kscottz * Add note for ROS users. Signed-off-by: kscottz * spelling errors. Signed-off-by: kscottz --------- Signed-off-by: Xùdōng Yáng Signed-off-by: kscottz Co-authored-by: Xùdōng Yáng --- README.rst | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/README.rst b/README.rst index 2bdf8aa000..a1f8b9c94f 100644 --- a/README.rst +++ b/README.rst @@ -30,6 +30,23 @@ configurations. .. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping +A Note for ROS Users +==================== + +**Cartographer is no longer actively maintained.** +On rare occasions critical pull requests may be merged, but no new development is currently taking place, including issue response. +If you are installing Cartographer in ROS 1 / ROS 2 using a binary package that package is a fork of this repository. +The ROS fork of Cartographer is only maintained in a limited capacity. +No new development takes place on this fork, but pull requests may be merged at the maintainers' discretion. + +The ROS fork of Cartographer, and the ROS wrapper library, can be found at these locations: + +- `Cartographer Fork `_ +- `Cartographer ROS `_ + +Additional discussion can be found in `this ROS Discourse thread`_. + + Getting started ===============