xray_points_processor.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
00018 #define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
00019 
00020 #include <map>
00021 
00022 #include "Eigen/Core"
00023 #include "cartographer/common/lua_parameter_dictionary.h"
00024 #include "cartographer/io/file_writer.h"
00025 #include "cartographer/io/points_processor.h"
00026 #include "cartographer/mapping/3d/hybrid_grid.h"
00027 #include "cartographer/mapping/detect_floors.h"
00028 #include "cartographer/mapping/proto/trajectory.pb.h"
00029 #include "cartographer/transform/rigid_transform.h"
00030 
00031 namespace cartographer {
00032 namespace io {
00033 
00034 // Creates X-ray cuts through the points with pixels being 'voxel_size' big.
00035 class XRayPointsProcessor : public PointsProcessor {
00036  public:
00037   constexpr static const char* kConfigurationFileActionName =
00038       "write_xray_image";
00039   enum class DrawTrajectories { kNo, kYes };
00040   XRayPointsProcessor(
00041       double voxel_size, double saturation_factor,
00042       const transform::Rigid3f& transform,
00043       const std::vector<mapping::Floor>& floors,
00044       const DrawTrajectories& draw_trajectories,
00045       const std::string& output_filename,
00046       const std::vector<mapping::proto::Trajectory>& trajectories,
00047       FileWriterFactory file_writer_factory, PointsProcessor* next);
00048 
00049   static std::unique_ptr<XRayPointsProcessor> FromDictionary(
00050       const std::vector<mapping::proto::Trajectory>& trajectories,
00051       FileWriterFactory file_writer_factory,
00052       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
00053 
00054   ~XRayPointsProcessor() override {}
00055 
00056   void Process(std::unique_ptr<PointsBatch> batch) override;
00057   FlushResult Flush() override;
00058 
00059   Eigen::AlignedBox3i bounding_box() const { return bounding_box_; }
00060 
00061  private:
00062   struct ColumnData {
00063     float sum_r = 0.;
00064     float sum_g = 0.;
00065     float sum_b = 0.;
00066     uint32_t count = 0;
00067   };
00068 
00069   struct Aggregation {
00070     mapping::HybridGridBase<bool> voxels;
00071     std::map<std::pair<int, int>, ColumnData> column_data;
00072   };
00073 
00074   void WriteVoxels(const Aggregation& aggregation,
00075                    FileWriter* const file_writer);
00076   void Insert(const PointsBatch& batch, Aggregation* aggregation);
00077 
00078   const DrawTrajectories draw_trajectories_;
00079   const std::vector<mapping::proto::Trajectory> trajectories_;
00080   FileWriterFactory file_writer_factory_;
00081   PointsProcessor* const next_;
00082 
00083   // If empty, we do not separate into floors.
00084   std::vector<mapping::Floor> floors_;
00085 
00086   const std::string output_filename_;
00087   const transform::Rigid3f transform_;
00088 
00089   // Only has one entry if we do not separate into floors.
00090   std::vector<Aggregation> aggregations_;
00091 
00092   // Bounding box containing all cells with data in all 'aggregations_'.
00093   Eigen::AlignedBox3i bounding_box_;
00094 
00095   // Scale the saturation of the point color. If saturation_factor_ > 1, the
00096   // point has darker color, otherwise it has lighter color.
00097   const double saturation_factor_;
00098 };
00099 
00100 }  // namespace io
00101 }  // namespace cartographer
00102 
00103 #endif  // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:36