diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc new file mode 100644 index 0000000..7c4347f --- /dev/null +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -0,0 +1,67 @@ +#include "cartographer/io/hybrid_grid_points_processor.h" + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/make_unique.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/sensor/range_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +HybridGridPointsProcessor::HybridGridPointsProcessor( + const double voxel_size, + const mapping_3d::proto::RangeDataInserterOptions& + range_data_inserter_options, + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), + range_data_inserter_(range_data_inserter_options), + hybrid_grid_(voxel_size), + file_writer_(std::move(file_writer)) {} + +std::unique_ptr +HybridGridPointsProcessor::FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetDouble("voxel_size"), + mapping_3d::CreateRangeDataInserterOptions( + dictionary->GetDictionary("range_data_inserter").get()), + file_writer_factory(dictionary->GetString("filename")), next); +} + +void HybridGridPointsProcessor::Process(std::unique_ptr batch) { + range_data_inserter_.Insert({batch->origin, batch->points, {}}, + &hybrid_grid_); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() { + const mapping_3d::proto::HybridGrid hybrid_grid_proto = + mapping_3d::ToProto(hybrid_grid_); + string serialized; + hybrid_grid_proto.SerializeToString(&serialized); + file_writer_->Write(serialized.data(), serialized.size()); + CHECK(file_writer_->Close()); + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "Hybrid grid generation must be configured to occur after " + "any stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL) << "Failed to receive FlushResult::kFinished"; +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/hybrid_grid_points_processor.h b/cartographer/io/hybrid_grid_points_processor.h new file mode 100644 index 0000000..bc25a8d --- /dev/null +++ b/cartographer/io/hybrid_grid_points_processor.h @@ -0,0 +1,55 @@ +#ifndef CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ + +// Library used for inserting range data points into a hybrid grid. + +#include +#include + +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" +#include "cartographer/mapping_3d/range_data_inserter.h" + +namespace cartographer { +namespace io { + +// Creates a hybrid grid of the points with voxels being 'voxel_size' +// big. 'range_data_inserter' options are used to configure the range +// data ray tracing through the hybrid grid. +class HybridGridPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "write_hybrid_grid"; + HybridGridPointsProcessor(double voxel_size, + const mapping_3d::proto::RangeDataInserterOptions& + range_data_inserter_options, + std::unique_ptr file_writer, + PointsProcessor* next); + HybridGridPointsProcessor(const HybridGridPointsProcessor&) = delete; + HybridGridPointsProcessor& operator=(const HybridGridPointsProcessor&) = + delete; + + static std::unique_ptr FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~HybridGridPointsProcessor() override {} + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + + mapping_3d::RangeDataInserter range_data_inserter_; + mapping_3d::HybridGrid hybrid_grid_; + std::unique_ptr file_writer_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 00bf224..53204ec 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -20,6 +20,7 @@ #include "cartographer/io/coloring_points_processor.h" #include "cartographer/io/counting_points_processor.h" #include "cartographer/io/fixed_ratio_sampling_points_processor.h" +#include "cartographer/io/hybrid_grid_points_processor.h" #include "cartographer/io/intensity_to_color_points_processor.h" #include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/io/null_points_processor.h" @@ -74,6 +75,8 @@ void RegisterBuiltInPointsProcessors( file_writer_factory, builder); RegisterFileWritingPointsProcessor( file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); // X-Ray is an odd ball since it requires the trajectory to figure out the // different building levels we walked on to separate the images.