cartographer源码分析(37)-io-pcd_writing_points_processor.h
来源:互联网 发布:网络教研室 编辑:程序博客网 时间:2024/06/06 15:37
源码可在https://github.com/learnmoreonce/SLAM 下载
文件:pcd_writing_points_processor.h#include <fstream>#include "cartographer/common/lua_parameter_dictionary.h"#include "cartographer/io/file_writer.h"#include "cartographer/io/points_processor.h"/*点云是目标表面特性的海量点集合。根据激光测量原理得到的点云,包括三维坐标(XYZ)和激光反射强度(Intensity)。根据摄影测量原理得到的点云,包括三维坐标(XYZ)和颜色信息(RGB)。结合激光测量和摄影测量原理得到点云,包括三维坐标(XYZ)、激光反射强度(Intensity)和颜色信息(RGB)。在获取物体表面每个采样点的空间坐标后,得到的是一个点的集合,称之为“点云”(Point Cloud)。pcd文件存储了每个点的x,y,z坐标以及r,g,b,a颜色信息*/namespace cartographer {namespace io {/*PcdWritingPointsProcessor是PointsProcessor的第九个子类(9).PcdWritingPointsProcessor类将点云按照pcd格式存储在pcd文件中.1,构造函数初始化一个文件类名,2,成员函数FromDictionary 从配置文件读取 文件名3,Process()和Flush()负责写入文件4,数据成员next_指向 下一阶段的PointsProcessor点云处理类*/// Streams a PCD file to disk. The header is written in 'Flush'.class PcdWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_pcd"; PcdWritingPointsProcessor(std::unique_ptr<FileWriter> file_writer, PointsProcessor* next); static std::unique_ptr<PcdWritingPointsProcessor> FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PcdWritingPointsProcessor() override {} PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete; PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) = delete; void Process(std::unique_ptr<PointsBatch> batch) override; FlushResult Flush() override; private: PointsProcessor* const next_; int64 num_points_; bool has_colors_; std::unique_ptr<FileWriter> file_writer_;};} // namespace io} // namespace cartographer
.
pcd_writing_points_processor.cc#include "cartographer/io/pcd_writing_points_processor.h"#include <iomanip>#include <sstream>#include <string>#include "cartographer/common/lua_parameter_dictionary.h"#include "cartographer/common/make_unique.h"#include "cartographer/io/points_batch.h"#include "glog/logging.h"namespace cartographer {namespace io {namespace {/*PCD文件由文件头和数据域组成,文件头存储了点云数据的字段格式信息,如: ======================================== # .PCD v0.7 - Point Cloud Data file format VERSION 0.7 FIELDS x y z rgba SIZE 4 4 4 4 TYPE F F F U COUNT 1 1 1 1 WIDTH 640 HEIGHT 480 VIEWPOINT 0 0 0 0 1 0 0 POINTS 307200 DATA binary //下面是点云数据段 123.1 21.1 64.0...======================================== */// Writes the PCD header claiming 'num_points' will follow it into// 'output_file'.void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, FileWriter* const file_writer) { string color_header_field = !has_color ? "" : " rgb"; string color_header_type = !has_color ? "" : " U"; string color_header_size = !has_color ? "" : " 4"; string color_header_count = !has_color ? "" : " 1"; std::ostringstream stream; stream << "# generated by Cartographer\n" << "VERSION .7\n" << "FIELDS x y z" << color_header_field << "\n" << "SIZE 4 4 4" << color_header_size << "\n" << "TYPE F F F" << color_header_type << "\n" << "COUNT 1 1 1" << color_header_count << "\n" << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n" << "HEIGHT 1\n" << "VIEWPOINT 0 0 0 1 0 0 0\n" << "POINTS " << std::setw(15) << std::setfill('0') << num_points << "\n" << "DATA binary\n"; const string out = stream.str(); file_writer->WriteHeader(out.data(), out.size());}//{x,y,z}写入文件void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, FileWriter* const file_writer) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); //64机,float:4位 memcpy(buffer + 8, &point[2], sizeof(float)); CHECK(file_writer->Write(buffer, 12));}void WriteBinaryPcdPointColor(const Color& color, FileWriter* const file_writer) { char buffer[4]; buffer[0] = color[2]; buffer[1] = color[1]; buffer[2] = color[0]; buffer[3] = 0; CHECK(file_writer->Write(buffer, 4));}} // namespacestd::unique_ptr<PcdWritingPointsProcessor>PcdWritingPointsProcessor::FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique<PcdWritingPointsProcessor>( file_writer_factory(dictionary->GetString("filename")), next);}PcdWritingPointsProcessor::PcdWritingPointsProcessor( std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), file_writer_(std::move(file_writer)) {}PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() { WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get()); //调用Flush()才写入head信息:此时才知道num_points_的值 CHECK(file_writer_->Close()); switch (next_->Flush()) { case FlushResult::kFinished: return FlushResult::kFinished; case FlushResult::kRestartStream://不能多次传递同一数据. LOG(FATAL) << "PCD generation must be configured to occur after any " "stages that require multiple passes."; } LOG(FATAL);}/*核心函数,按照{x,y,z}写入文件*/void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) { if (batch->points.empty()) { next_->Process(std::move(batch)); return; } if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get()); } for (size_t i = 0; i < batch->points.size(); ++i) { WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get()); if (!batch->colors.empty()) { WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get()); } ++num_points_;//统计处理了多少points } next_->Process(std::move(batch));}} // namespace io} // namespace cartographer
本文发于:
* http://www.jianshu.com/u/9e38d2febec1
* https://zhuanlan.zhihu.com/learnmoreonce
* http://blog.csdn.net/learnmoreonce
* slam源码分析微信公众号:slamcode
阅读全文
0 0
- cartographer源码分析(37)-io-pcd_writing_points_processor.h
- cartographer源码分析(26)-io-file_writer.h
- cartographer源码分析(27)-io-points_batch.h
- cartographer源码分析(28)-io-points_processor.h
- cartographer源码分析(29)-io-min_max_range_filtering_points_processor.h
- cartographer源码分析(30)-io-counting_points_processor.h
- cartographer源码分析(31)-io-xray_points_processor.h
- cartographer源码分析(32)-io-intensity_to_color_points_processor.h
- cartographer源码分析(33)-io-ply_writing_points_processor.h
- cartographer源码分析(34)-io-coloring_points_processor.h
- cartographer源码分析(35)-io-fixed_ratio_sampling_points_processor.h
- cartographer源码分析(36)-io- outlier_removing_points_processor.h
- cartographer源码分析(38)-io-null_points_processor.h
- cartographer源码分析(3)-common-port.h
- cartographer源码分析(4)-common-time.h
- cartographer源码分析(5)-common-rate_time.h
- cartographer源码分析(6)-common-histogram.h
- cartographer源码分析(7)-common-math.h
- 2017年7月29日22:06:41
- Matlab_回归分析第二讲之Logistic回归
- css实现背景颜色透明,文字不透明
- Step 4:Processing基本函数绘制图形
- JavaScript学习--基本语法
- cartographer源码分析(37)-io-pcd_writing_points_processor.h
- linux中的网络管理
- hdu 2075 A|B?
- 收集一些好问题
- 2017多校联合一1002/hdu6034Balala Power!(搜索,贪心,排序,大数进制)
- cartographer源码分析(38)-io-null_points_processor.h
- C语言数据结构-栈-数组实现
- [第五季]7.HTML列表和表单标签
- 多校连萌 【赛后记录】