104 lines
3.4 KiB
C++
Executable File
104 lines
3.4 KiB
C++
Executable File
/*
|
|
* 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 <map>
|
|
|
|
#include "Eigen/Core"
|
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
|
#include "cartographer/io/file_writer.h"
|
|
#include "cartographer/io/points_processor.h"
|
|
#include "cartographer/mapping/3d/hybrid_grid.h"
|
|
#include "cartographer/mapping/detect_floors.h"
|
|
#include "cartographer/mapping/proto/trajectory.pb.h"
|
|
#include "cartographer/transform/rigid_transform.h"
|
|
|
|
namespace cartographer {
|
|
namespace io {
|
|
|
|
// Creates X-ray cuts through the points with pixels being 'voxel_size' big.
|
|
class XRayPointsProcessor : public PointsProcessor {
|
|
public:
|
|
constexpr static const char* kConfigurationFileActionName =
|
|
"write_xray_image";
|
|
enum class DrawTrajectories { kNo, kYes };
|
|
XRayPointsProcessor(
|
|
double voxel_size, double saturation_factor,
|
|
const transform::Rigid3f& transform,
|
|
const std::vector<mapping::Floor>& floors,
|
|
const DrawTrajectories& draw_trajectories,
|
|
const std::string& output_filename,
|
|
const std::vector<mapping::proto::Trajectory>& trajectories,
|
|
FileWriterFactory file_writer_factory, PointsProcessor* next);
|
|
|
|
static std::unique_ptr<XRayPointsProcessor> FromDictionary(
|
|
const std::vector<mapping::proto::Trajectory>& trajectories,
|
|
FileWriterFactory file_writer_factory,
|
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
|
|
|
~XRayPointsProcessor() override {}
|
|
|
|
void Process(std::unique_ptr<PointsBatch> batch) override;
|
|
FlushResult Flush() override;
|
|
|
|
Eigen::AlignedBox3i bounding_box() const { return bounding_box_; }
|
|
|
|
private:
|
|
struct ColumnData {
|
|
float sum_r = 0.;
|
|
float sum_g = 0.;
|
|
float sum_b = 0.;
|
|
uint32_t count = 0;
|
|
};
|
|
|
|
struct Aggregation {
|
|
mapping::HybridGridBase<bool> voxels;
|
|
std::map<std::pair<int, int>, ColumnData> column_data;
|
|
};
|
|
|
|
void WriteVoxels(const Aggregation& aggregation,
|
|
FileWriter* const file_writer);
|
|
void Insert(const PointsBatch& batch, Aggregation* aggregation);
|
|
|
|
const DrawTrajectories draw_trajectories_;
|
|
const std::vector<mapping::proto::Trajectory> trajectories_;
|
|
FileWriterFactory file_writer_factory_;
|
|
PointsProcessor* const next_;
|
|
|
|
// If empty, we do not separate into floors.
|
|
std::vector<mapping::Floor> floors_;
|
|
|
|
const std::string output_filename_;
|
|
const transform::Rigid3f transform_;
|
|
|
|
// Only has one entry if we do not separate into floors.
|
|
std::vector<Aggregation> aggregations_;
|
|
|
|
// 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
|
|
} // namespace cartographer
|
|
|
|
#endif // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
|