diff --git a/CMakeLists.txt b/CMakeLists.txt index 38b6733..6379c39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,15 @@ set_source_files_properties(${ALL_PROTO_SRCS} ${ALL_PROTO_HDRS} PROPERTIES GENER list(APPEND ALL_SRCS ${ALL_PROTO_SRCS} ${ALL_PROTO_HDRS}) add_library(${PROJECT_NAME} ${ALL_SRCS}) -add_subdirectory("cartographer") + +configure_file( + ${PROJECT_SOURCE_DIR}/cartographer/common/config.h.cmake + ${PROJECT_BINARY_DIR}/cartographer/common/config.h) + +google_binary(cartographer_compute_relations_metrics + SRCS + cartographer/ground_truth/compute_relations_metrics_main.cc +) foreach(ABS_FIL ${ALL_TESTS}) file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${ABS_FIL}) diff --git a/cartographer/CMakeLists.txt b/cartographer/CMakeLists.txt deleted file mode 100644 index 0fc79b8..0000000 --- a/cartographer/CMakeLists.txt +++ /dev/null @@ -1,16 +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. - -add_subdirectory("common") -add_subdirectory("ground_truth") diff --git a/cartographer/common/CMakeLists.txt b/cartographer/common/CMakeLists.txt deleted file mode 100644 index 1c59fb8..0000000 --- a/cartographer/common/CMakeLists.txt +++ /dev/null @@ -1,17 +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. - -configure_file ( - ${CMAKE_CURRENT_SOURCE_DIR}/config.h.cmake - ${CMAKE_CURRENT_BINARY_DIR}/config.h) diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc index b35fddf..0a2464e 100644 --- a/cartographer/common/configuration_files_test.cc +++ b/cartographer/common/configuration_files_test.cc @@ -55,8 +55,9 @@ TEST(ConfigurationFilesTest, ValidateTrajectoryBuilderOptions) { ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( kCode, std::move(file_resolver)); ::cartographer::mapping::CreateTrajectoryBuilderOptions( - &lua_parameter_dictionary); - }); + &lua_parameter_dictionary); + }); } + } // namespace } // namespace cartographer_ros diff --git a/cartographer/common/interval.h b/cartographer/common/interval.h deleted file mode 100644 index 4c5a81c..0000000 --- a/cartographer/common/interval.h +++ /dev/null @@ -1,32 +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. - */ - -#ifndef CARTOGRAPHER_COMMON_INTERVAL_H_ -#define CARTOGRAPHER_COMMON_INTERVAL_H_ - -namespace cartographer { -namespace common { - -template -struct Interval { - T start; - T end; -}; - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_INTERVAL_H_ diff --git a/cartographer/ground_truth/CMakeLists.txt b/cartographer/ground_truth/CMakeLists.txt deleted file mode 100644 index 918fc4d..0000000 --- a/cartographer/ground_truth/CMakeLists.txt +++ /dev/null @@ -1,18 +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. - -google_binary(cartographer_compute_relations_metrics - SRCS - compute_relations_metrics_main.cc -) diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 36f418c..902a8ac 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -120,11 +120,10 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) { CHECK(file_writer->Close()); } -bool ContainedIn( - const common::Time& time, - const std::vector>& time_intervals) { - for (const auto& interval : time_intervals) { - if (interval.start <= time && time <= interval.end) { +bool ContainedIn(const common::Time& time, + const std::vector& timespans) { + for (const mapping::Timespan& timespan : timespans) { + if (timespan.start <= time && time <= timespan.end) { return true; } } diff --git a/cartographer/mapping/detect_floors.cc b/cartographer/mapping/detect_floors.cc index 3551eb9..87fa1ea 100644 --- a/cartographer/mapping/detect_floors.cc +++ b/cartographer/mapping/detect_floors.cc @@ -39,14 +39,15 @@ constexpr double kMaxShortSpanLengthMeters = 25.; constexpr double kLevelHeightMeters = 2.5; constexpr double kMinLevelSeparationMeters = 1.0; -// Indices into 'trajectory.node', so that index.start <= i < index.end. +// Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'. struct Span { - common::Interval index; + int start_index; + int end_index; std::vector z_values; bool operator<(const Span& other) const { - return std::forward_as_tuple(index.start, index.end) < - std::forward_as_tuple(other.index.start, other.index.end); + return std::forward_as_tuple(start_index, end_index) < + std::forward_as_tuple(other.start_index, other.end_index); } }; @@ -81,15 +82,15 @@ std::vector SliceByAltitudeChange(const proto::Trajectory& trajectory) { CHECK_GT(trajectory.node_size(), 0); std::vector spans; - spans.push_back(Span{{0, 0}, {trajectory.node(0).pose().translation().z()}}); + spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}}); for (int i = 1; i < trajectory.node_size(); ++i) { const auto& node = trajectory.node(i); const double z = node.pose().translation().z(); if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) { - spans.push_back(Span{{i, i}, {}}); + spans.push_back(Span{i, i, {}}); } InsertSorted(z, &spans.back().z_values); - spans.back().index.end = i + 1; + spans.back().end_index = i + 1; } return spans; } @@ -97,7 +98,7 @@ std::vector SliceByAltitudeChange(const proto::Trajectory& trajectory) { // Returns the length of 'span' in meters. double SpanLength(const proto::Trajectory& trajectory, const Span& span) { double length = 0; - for (int i = span.index.start + 1; i < span.index.end; ++i) { + for (int i = span.start_index + 1; i < span.end_index; ++i) { const auto a = transform::ToEigen(trajectory.node(i - 1).pose().translation()); const auto b = transform::ToEigen(trajectory.node(i).pose().translation()); @@ -182,10 +183,10 @@ std::vector FindFloors(const proto::Trajectory& trajectory, z_values.insert(z_values.end(), span.z_values.begin(), span.z_values.end()); } - floors.back().timespans.push_back(common::Interval{ - common::FromUniversal(trajectory.node(span.index.start).timestamp()), + floors.back().timespans.push_back(Timespan{ + common::FromUniversal(trajectory.node(span.start_index).timestamp()), common::FromUniversal( - trajectory.node(span.index.end - 1).timestamp())}); + trajectory.node(span.end_index - 1).timestamp())}); } std::sort(z_values.begin(), z_values.end()); floors.back().z = Median(z_values); diff --git a/cartographer/mapping/detect_floors.h b/cartographer/mapping/detect_floors.h index 0f98843..189086f 100644 --- a/cartographer/mapping/detect_floors.h +++ b/cartographer/mapping/detect_floors.h @@ -19,17 +19,21 @@ #include "cartographer/mapping/proto/trajectory.pb.h" -#include "cartographer/common/interval.h" #include "cartographer/common/time.h" namespace cartographer { namespace mapping { +struct Timespan { + common::Time start; + common::Time end; +}; + struct Floor { // The spans of time we spent on this floor. Since we might have walked up and // down many times in this place, there can be many spans of time we spent on // a particular floor. - std::vector> timespans; + std::vector timespans; // The median z-value of this floor. double z; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index f8d5b5a..eb03e2d 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -26,7 +26,6 @@ #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/mapping_2d/global_trajectory_builder.h" #include "cartographer/mapping_3d/global_trajectory_builder.h" -#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" @@ -52,18 +51,6 @@ proto::MapBuilderOptions CreateMapBuilderOptions( return options; } -proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::TrajectoryBuilderOptions options; - *options.mutable_trajectory_builder_2d_options() = - mapping_2d::CreateLocalTrajectoryBuilderOptions( - parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); - *options.mutable_trajectory_builder_3d_options() = - mapping_3d::CreateLocalTrajectoryBuilderOptions( - parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); - return options; -} - MapBuilder::MapBuilder( const proto::MapBuilderOptions& options, std::deque* const constant_data) diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 8e29935..56fb5d5 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -44,8 +44,6 @@ namespace mapping { proto::MapBuilderOptions CreateMapBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary); -proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary); // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // and a SparsePoseGraph for loop closure. diff --git a/cartographer/mapping/trajectory_builder.cc b/cartographer/mapping/trajectory_builder.cc new file mode 100644 index 0000000..2616b2f --- /dev/null +++ b/cartographer/mapping/trajectory_builder.cc @@ -0,0 +1,38 @@ +/* + * 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/trajectory_builder.h" + +#include "cartographer/mapping_2d/local_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options.h" + +namespace cartographer { +namespace mapping { + +proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::TrajectoryBuilderOptions options; + *options.mutable_trajectory_builder_2d_options() = + mapping_2d::CreateLocalTrajectoryBuilderOptions( + parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); + *options.mutable_trajectory_builder_3d_options() = + mapping_3d::CreateLocalTrajectoryBuilderOptions( + parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 8c870b2..ecec1dc 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -21,9 +21,11 @@ #include #include +#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/data.h" #include "cartographer/sensor/point_cloud.h" @@ -32,6 +34,9 @@ namespace cartographer { namespace mapping { +proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + // This interface is used for both 2D and 3D SLAM. class TrajectoryBuilder { public: