diff --git a/CMakeLists.txt b/CMakeLists.txt index d79f50a..4308394 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,11 +25,14 @@ include("${CMAKE_SOURCE_DIR}/cmake/functions.cmake") google_initialize_cartographer_project() google_enable_testing() +include(FindPkgConfig) + find_package(Boost REQUIRED COMPONENTS system iostreams) find_package(Ceres REQUIRED) find_package(Eigen3 REQUIRED) find_package(LuaGoogle REQUIRED) find_package(Protobuf REQUIRED) +PKG_SEARCH_MODULE(CAIRO REQUIRED cairo>=1.12.16) # Only build the documentation if we can find Sphinx. find_package(Sphinx) @@ -83,6 +86,7 @@ list(APPEND CARTOGRAPHER_LIBRARIES "${CERES_LIBRARIES}") list(APPEND CARTOGRAPHER_LIBRARIES "${Boost_LIBRARIES}") list(APPEND CARTOGRAPHER_LIBRARIES "${LUA_LIBRARIES}") list(APPEND CARTOGRAPHER_LIBRARIES "${PROTOBUF_LIBRARIES}") +list(APPEND CARTOGRAPHER_LIBRARIES "${CAIRO_LIBRARIES}") list(APPEND CARTOGRAPHER_LIBRARIES "webp") CONFIGURE_PACKAGE_CONFIG_FILE( diff --git a/cartographer/CMakeLists.txt b/cartographer/CMakeLists.txt index 9982b30..c149ccf 100644 --- a/cartographer/CMakeLists.txt +++ b/cartographer/CMakeLists.txt @@ -13,6 +13,7 @@ # limitations under the License. add_subdirectory("common") +add_subdirectory("io") add_subdirectory("kalman_filter") add_subdirectory("mapping") add_subdirectory("mapping_2d") diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt new file mode 100644 index 0000000..73e9e4c --- /dev/null +++ b/cartographer/io/CMakeLists.txt @@ -0,0 +1,42 @@ +google_library(io_cairo_types + USES_CAIRO + HDRS + cairo_types.h +) + +google_library(io_null_points_processor + HDRS + null_points_processor.h + DEPENDS + io_points_processor +) + +google_library(io_points_batch + USES_EIGEN + HDRS + points_batch.h + DEPENDS + common_time +) + +google_library(io_points_processor + HDRS + points_processor.h + DEPENDS + io_points_batch +) + +google_library(io_xray_points_processor + USES_CAIRO + USES_EIGEN + SRCS + xray_points_processor.cc + HDRS + xray_points_processor.h + DEPENDS + common_math + io_cairo_types + io_points_processor + mapping_3d_hybrid_grid + transform_rigid_transform +) diff --git a/cartographer/io/cairo_types.h b/cartographer/io/cairo_types.h new file mode 100644 index 0000000..568df4b --- /dev/null +++ b/cartographer/io/cairo_types.h @@ -0,0 +1,29 @@ +#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_ +#define CARTOGRAPHER_IO_CAIRO_TYPES_H_ + +#include + +#include "cairo/cairo.h" + +namespace cartographer { +namespace io { +namespace cairo { + +// std::unique_ptr for Cairo surfaces. The surface is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueSurfacePtr = + std::unique_ptr; + +// std::unique_ptr for Cairo contexts. The context is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueContextPtr = std::unique_ptr; + +// std::unique_ptr for Cairo paths. The path is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniquePathPtr = std::unique_ptr; + +} // namespace cairo +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_ diff --git a/cartographer/io/null_points_processor.h b/cartographer/io/null_points_processor.h new file mode 100644 index 0000000..03cc3d9 --- /dev/null +++ b/cartographer/io/null_points_processor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ + +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// A points processor that just drops all points. The end of a pipeline usually. +class NullPointsProcessor : public PointsProcessor { + public: + NullPointsProcessor() {} + ~NullPointsProcessor() override {} + + void Process(const PointsBatch& points_batch) override {} + void Flush() override {} +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h new file mode 100644 index 0000000..caf98d3 --- /dev/null +++ b/cartographer/io/points_batch.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_POINTS_BATCH_H_ +#define CARTOGRAPHER_IO_POINTS_BATCH_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/common/time.h" + +namespace cartographer { +namespace io { + +// A number of points, captured around the same 'time' and by a +// sensor at the same 'origin'. +struct PointsBatch { + PointsBatch() { + origin = Eigen::Vector3f::Zero(); + trajectory_index = 0; + } + + // Time at which this batch has been acquired. + common::Time time; + + // Origin of the data, i.e. the location of the sensor in the world at + // 'time'. + Eigen::Vector3f origin; + + // Sensor that generated this data's 'frame_id' or empty if this information + // is unknown. + string frame_id; + + // Trajectory index that produced this point. + int trajectory_index; + + std::vector points; + std::vector normals; + std::vector> colors; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_POINTS_BATCH_H_ diff --git a/cartographer/io/points_processor.h b/cartographer/io/points_processor.h new file mode 100644 index 0000000..618fe43 --- /dev/null +++ b/cartographer/io/points_processor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ + +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +// A processor in a pipeline. It processes a 'points_batch' and hands it to the +// next processor in the pipeline. Once 'flush' is called no more data will be +// send through the pipeline. +class PointsProcessor { + public: + PointsProcessor() {} + virtual ~PointsProcessor() {} + + PointsProcessor(const PointsProcessor&) = delete; + PointsProcessor& operator=(const PointsProcessor&) = delete; + + virtual void Process(const PointsBatch& points_batch) = 0; + virtual void Flush() = 0; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc new file mode 100644 index 0000000..b3814fc --- /dev/null +++ b/cartographer/io/xray_points_processor.cc @@ -0,0 +1,134 @@ +/* + * 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/xray_points_processor.h" + +#include +#include + +#include "Eigen/Core" +#include "cairo/cairo.h" +#include "cartographer/common/math.h" +#include "cartographer/io/cairo_types.h" +#include "cartographer/mapping_3d/hybrid_grid.h" + +namespace cartographer { +namespace io { +namespace { + +// Takes the logarithm of each value in 'mat', clamping to 0 as smallest value. +void TakeLogarithm(Eigen::MatrixXf* mat) { + for (int y = 0; y < mat->rows(); ++y) { + for (int x = 0; x < mat->cols(); ++x) { + const float value = (*mat)(y, x); + if (value == 0.f) { + continue; + } + const float new_value = std::log(value); + (*mat)(y, x) = new_value; + } + } +} + +// Write 'mat' as a pleasing-to-look-at PNG into 'filename' +void WritePng(const string& filename, const Eigen::MatrixXf& mat) { + const int stride = + cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols()); + std::vector pixels(stride * mat.rows(), 0.); + + const float max = mat.maxCoeff(); + for (int y = 0; y < mat.rows(); ++y) { + for (int x = 0; x < mat.cols(); ++x) { + const float value = mat(y, x); + uint8_t shade = common::RoundToInt(255.f * (1.f - value / max)); + pixels[y * stride / 4 + x] = + (255 << 24) | (shade << 16) | (shade << 8) | shade; + } + } + + // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of + // the data until the surface is finalized. Once it is finalized though, + // cairo_surface_write_to_png fails, complaining that the surface is already + // finalized. This makes it pretty hard to pass back ownership of the image to + // the caller. + cairo::UniqueSurfacePtr surface( + cairo_image_surface_create_for_data( + reinterpret_cast(pixels.data()), CAIRO_FORMAT_ARGB32, + mat.cols(), mat.rows(), stride), + cairo_surface_destroy); + cairo_surface_write_to_png(surface.get(), filename.c_str()); +} + +} // namespace + +XRayPointsProcessor::XRayPointsProcessor(const double voxel_size, + const transform::Rigid3f& transform, + const string& output_filename, + PointsProcessor* next) + : next_(next), + output_filename_(output_filename), + transform_(transform), + voxels_(voxel_size, Eigen::Vector3f::Zero()) {} + +void XRayPointsProcessor::Process(const PointsBatch& batch) { + for (const auto& point : batch.points) { + const Eigen::Vector3f camera_point = transform_ * point; + *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true; + } + next_->Process(batch); +} + +void XRayPointsProcessor::Flush() { + WriteImage(); + return next_->Flush(); +} + +void XRayPointsProcessor::WriteImage() { + Eigen::Array3i min(std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()); + Eigen::Array3i max(std::numeric_limits::min(), + std::numeric_limits::min(), + std::numeric_limits::min()); + + // Find the maximum and minimum cells. + for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) { + const Eigen::Array3i idx = it.GetCellIndex(); + min = min.min(idx); + max = max.max(idx); + } + + // Returns the (x, y) pixel of the given 'index'. + const auto voxel_index_to_pixel = [&max, &min](const Eigen::Array3i& index) { + // We flip the y axis, since matrices rows are counted from the top. + return Eigen::Array2i(max[1] - index[1], max[2] - index[2]); + }; + + // Hybrid grid uses X: forward, Y: left, Z: up. + // For the screen we are using. X: right, Y: up + const int xsize = max[1] - min[1] + 1; + const int ysize = max[2] - min[2] + 1; + Eigen::MatrixXf image = Eigen::MatrixXf::Zero(ysize, xsize); + for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) { + const Eigen::Array2i pixel = voxel_index_to_pixel(it.GetCellIndex()); + ++image(pixel.y(), pixel.x()); + } + TakeLogarithm(&image); + WritePng(output_filename_, image); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h new file mode 100644 index 0000000..200c03b --- /dev/null +++ b/cartographer/io/xray_points_processor.h @@ -0,0 +1,55 @@ +/* + * 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_XRAY_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ + +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace io { + +// Creates X-ray cuts through the points with pixels being 'voxel_size' big. All +// images created from a single XRayPointsProcessor have the same dimensions and +// are centered on the center of the bounding box, so that they can easily be +// combined into a movie. +class XRayPointsProcessor : public PointsProcessor { + public: + XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform, + const string& output_filename, PointsProcessor* next); + + ~XRayPointsProcessor() override {} + + void Process(const PointsBatch& batch) override; + void Flush() override; + + private: + using Voxels = mapping_3d::HybridGridBase; + + void WriteImage(); + + PointsProcessor* const next_; + const string output_filename_; + const transform::Rigid3f transform_; + Voxels voxels_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ diff --git a/cmake/functions.cmake b/cmake/functions.cmake index 0bb1979..27f0beb 100644 --- a/cmake/functions.cmake +++ b/cmake/functions.cmake @@ -24,6 +24,7 @@ macro(_parse_arguments ARGS) USES_GLOG USES_LUA USES_WEBP + USES_CAIRO ) # Options only used by projects using Cartographers cmake files. @@ -106,6 +107,12 @@ macro(_common_compile_stuff VISIBILITY) target_link_libraries("${NAME}" yaml-cpp) endif() + if(ARG_USES_CAIRO) + target_include_directories("${NAME}" SYSTEM ${VISIBILITY} + "${CAIRO_INCLUDE_DIRS}") + target_link_libraries("${NAME}" ${CAIRO_LIBRARIES}) + endif() + set_target_properties(${NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index 007b3e7..e724859 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -25,6 +25,7 @@ sudo apt-get install -y \ git \ google-mock \ libboost-all-dev \ + libcairo2-dev \ libeigen3-dev \ libgflags-dev \ libgoogle-glog-dev \ diff --git a/scripts/update_cmakelists.py b/scripts/update_cmakelists.py index 18cedd7..0c99ac4 100755 --- a/scripts/update_cmakelists.py +++ b/scripts/update_cmakelists.py @@ -114,6 +114,8 @@ def ExtractUses(project_name, source): uses.add("USES_ROS") if re.match(r'^#include ["<]yaml-cpp/', line): uses.add("USES_YAMLCPP") + if re.match(r'^#include ["<]cairo/', line): + uses.add("USES_CAIRO") if project_name != "cartographer": if re.match(r'^#include ["<]cartographer/', line): uses.add("USES_CARTOGRAPHER")