Skip to content

Instantly share code, notes, and snippets.

@max-dark
Last active Nov 20, 2020
Embed
What would you like to do?
#include "buffer.hxx"
#include <filesystem>
#include <fstream>
#include <ios>
namespace fs = std::filesystem;
namespace Scan
{
constexpr auto points_per_line = 1024u;
constexpr auto num_lines = 512u;
constexpr auto points_count = 1024u * 512u;
constexpr auto marks_per_line = 5u;
constexpr auto f_size = points_count * sizeof(float);
constexpr auto m_size = marks_per_line * sizeof(uint32_t);
constexpr auto x_offset = 0u;
constexpr auto y_offset = x_offset + f_size;
constexpr auto m_offset = y_offset + f_size;
constexpr auto i_offset = m_offset + m_size;
static_assert(x_offset == 0u);
static_assert(y_offset == (points_count * sizeof(float)));
[[nodiscard]] const void* getDataAt(const Buffer* buffer, size_t offset)
{
return buffer->data.data() + offset;
}
template <typename Type>
const Type* getPointer(const Buffer * buffer, size_t offset, size_t line, size_t perLine)
{
auto raw_ptr = getDataAt(buffer, offset);
auto ptr = reinterpret_cast<const Type*>(raw_ptr);
return ptr + line * perLine;
}
template <typename Type>
const Type* getPointer(const Buffer * buffer, size_t offset, size_t line)
{
return getPointer<Type>(buffer, offset, line, buffer->pointsPerLine());
}
const float *Buffer::getX(size_t line) const
{
return getPointer<float>(this, x_offset, line);
}
const float *Buffer::getY(size_t line) const
{
return getPointer<float>(this, y_offset, line);
}
const uint32_t *Buffer::getMark(size_t line) const
{
return getPointer<uint32_t>(this, m_offset, line, marks_per_line);
}
const uint8_t *Buffer::getIntensity(size_t line) const
{
return getPointer<uint8_t>(this, i_offset, line);
}
size_t Buffer::pointsPerLine() const
{
return points_per_line;
}
size_t Buffer::numLines() const
{
return num_lines;
}
void Buffer::swap(Buffer & other)
{
std::swap(data, other.data);
}
bool Buffer::enable(size_t line) const
{
auto m = getMark(line);
auto mark = Mark::from_data(m);
return mark.encoder_enable();
}
Optional loadBuffer(const std::string &file)
{
if (const fs::path file_path{file}; fs::exists(file_path))
{
Buffer temp;
auto file_size = fs::file_size(file_path);
temp.data.resize(file_size);
{
std::ifstream data;
data.open(file_path, std::ios::binary | std::ios::in);
if (data.is_open())
{
if (data.read(temp.data.data(), file_size))
{
return temp;
}
}
}
}
return {};
}
bool Mark::encoder_enable() const
{
return 0 != (m_status & ENCODER_ENABLE);
}
bool Mark::encoder_a_phase() const
{
return 0 != (m_status & ENCODER_A);
}
bool Mark::encoder_b_phase() const
{
return 0 != (m_status & ENCODER_B);
}
uint8_t Mark::over_trig() const
{
return (m_status & OVER_TRIG_MASK) >> OVER_TRIG_SHIFT;
}
Mark Mark::from_data(const uint32_t *data)
{
Mark m{};
m.m_value = data[VALUE];
m.m_status = data[STATUS];
m.m_sample_timestamp = data[SAMPLE];
m.m_encoder_timestamp = data[ENCODER];
m.m_scan_id = data[SCAN_ID];
return m;
}
uint32_t Mark::value() const
{
return m_value;
}
uint32_t Mark::sample() const
{
return m_sample_timestamp;
}
uint32_t Mark::encoder() const
{
return m_encoder_timestamp;
}
uint32_t Mark::scanId() const
{
return m_scan_id;
}
} // namespace Scan
#ifndef POINT3D__BUFFER_HXX
#define POINT3D__BUFFER_HXX
#include <vector>
#include <string>
#include <optional>
namespace Scan
{
using std::size_t;
struct Mark
{
enum Mask
{
BOOL_MASK = 1u,
BYTE_MASK = 0xFFu,
ENCODER_ENABLE = BOOL_MASK << 30u,
ENCODER_A = BOOL_MASK << 28u,
ENCODER_B = BOOL_MASK << 27u,
OVER_TRIG_SHIFT = 8u,
OVER_TRIG_MASK = BYTE_MASK << OVER_TRIG_SHIFT,
};
enum Index
{
VALUE,
STATUS,
SAMPLE,
ENCODER,
SCAN_ID,
};
uint32_t value() const;
uint32_t sample() const;
uint32_t encoder() const;
uint32_t scanId() const;
[[nodiscard]] bool encoder_enable() const;
[[nodiscard]] bool encoder_a_phase() const;
[[nodiscard]] bool encoder_b_phase() const;
[[nodiscard]] uint8_t over_trig() const;
static Mark from_data(const uint32_t* data);
private:
uint32_t m_value;
uint32_t m_status;
uint32_t m_sample_timestamp;
uint32_t m_encoder_timestamp;
uint32_t m_scan_id;
};
struct Buffer
{
std::vector<char> data;
[[nodiscard]] const float * getX(size_t line) const;
[[nodiscard]] const float * getY(size_t line) const;
[[nodiscard]] const uint32_t * getMark(size_t line) const;
[[nodiscard]] const uint8_t * getIntensity(size_t line) const;
[[nodiscard]] size_t pointsPerLine() const;
[[nodiscard]] size_t numLines() const;
[[nodiscard]] bool enable(size_t line) const;
void swap(Buffer& other);
};
using Optional = std::optional<Buffer>;
Optional loadBuffer(const std::string& file);
} // namespace Scan
#endif //POINT3D__BUFFER_HXX
#include <iostream>
#include "buffer.hxx"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/angles.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <cmath>
#include <map>
using Point = pcl::PointXYZI;
using Cloud = pcl::PointCloud<Point>;
// эти значения получены из документации к камерам
constexpr auto Hplane = 1280.0f;
constexpr auto Xcamera = 1000.0f;
constexpr auto Zcamera = 2054.0f;
constexpr auto Zcenter = Zcamera - 2.0f*Hplane/3.0f;
auto from_degree(float angle)
{
return pcl::deg2rad(angle);
}
auto make_transform(float angle, float dx, float dy)
{
using namespace Eigen;
auto axis = Eigen::Vector3f::UnitZ();
Eigen::Vector3f center{};
center.x() = Xcamera;
center.y() = Zcenter;
center.z() = 0.0f;
// вращение вокруг центра треугольника камер
auto rotation = Translation3f(center) * AngleAxisf(angle, axis) * Translation3f(-center);
// добавить смещение
auto total = rotation.matrix();
total(0, 3) += dx;
total(1, 3) += dy;
return total;
}
auto do_transform(const Cloud& source, const Eigen::Matrix4f& transform)
{
auto target = std::make_shared<Cloud>();
pcl::transformPointCloud(source, *target, transform);
return target;
}
auto do_transform(const Cloud& source, float angle, float dx, float dy)
{
auto transform = make_transform(angle, dx, dy);
return do_transform(source, transform);
}
auto loadCloud(const std::string& file_name)
{
auto cloud = std::make_shared<Cloud>();
pcl::io::loadPCDFile(file_name, *cloud);
return cloud;
}
auto loadBuffer(const std::string& file_name)
{
auto buffer = Scan::loadBuffer(file_name);
auto cloud = std::make_shared<Cloud>();
cloud->width = buffer->pointsPerLine();
cloud->height = buffer->numLines();
cloud->is_dense = false;
cloud->reserve(cloud->width * cloud->height);
auto total = 0u;
for (auto line = 0u; line < buffer->numLines(); ++line)
{
auto x_line = buffer->getX(line);
auto y_line = buffer->getY(line);
auto i_line = buffer->getIntensity(line);
auto z = line * 10.f; // одно сечение каждые 100 импульсов энкодера (~10 милиметров)
if (buffer->enable(line)) // датчик наличия сработал?
{
++total; // посчитаем количество
}
for (auto idx = 0u; idx < buffer->pointsPerLine(); ++idx)
{
auto x = x_line[idx];
auto y = y_line[idx];
auto i = i_line[idx];
Point pt{};
pt.x = pt.y = pt.y = pt.intensity = NAN;
if (x > 0.0 && y > 0.0) // точки [0, 0] означают бесконечность
{
pt.x = x; pt.y = y; pt.z = z;
pt.intensity = i;
}
cloud->points.push_back(pt);
}
}
std::cout << "file " << file_name << " enabled: " << total << std::endl;
return cloud;
}
using Viewer = pcl::visualization::PCLVisualizer;
using Color = pcl::visualization::PointCloudColorHandlerCustom<Point>;
using Grid = pcl::VoxelGrid<Point>;
using RangeAnd = pcl::ConditionAnd<Point>;
using PlaneAnd = pcl::FieldComparison<Point>;
using Condition = pcl::ConditionalRemoval<Point>;
using RadiusFilter = pcl::RadiusOutlierRemoval<Point>;
using Statistic = pcl::StatisticalOutlierRemoval<Point>;
/// условие обрезки
auto make_roi(float left, float bottom, float right, float top)
{
namespace Ops = pcl::ComparisonOps;
auto range = std::make_shared<RangeAnd>();
auto p_b = std::make_shared<const PlaneAnd>("y", Ops::GE, bottom);
auto p_t = std::make_shared<const PlaneAnd>("y", Ops::LE, top);
auto p_l = std::make_shared<const PlaneAnd>("x", Ops::GE, left);
auto p_r = std::make_shared<const PlaneAnd>("x", Ops::LE, right);
// плоскости отсечения
range->addComparison(p_b); // нижняя
range->addComparison(p_t); // верхняя
range->addComparison(p_l); // левая
range->addComparison(p_r); // правая
return range;
}
auto make_roi()
{
return make_roi(700, 700, 1500, 1500);
}
auto make_roi(float x, float y, float side)
{
auto half = side / 2.0f;
return make_roi(x - half, y - half, x + half, y + half);
}
int main(int argc, char** argv)
{
// марицы совмещения камер(подобрано на глаз, нужно уточнение)
auto transform_t = make_transform(from_degree(0), 0, 0);
auto transform_l = make_transform(from_degree(-120), -20, 50);
auto transform_r = make_transform(from_degree(+120), +15, 50);
if (argc == 2)
{
const std::string base_name = argv[1];
const std::string ext = ".dat";
const std::string sides[] = {"_left", "_top", "_right"};
auto [side_left, side_top, side_right] = sides;
std::map<std::string, Cloud> points;
std::map<std::string, Eigen::Matrix4f> transforms;
transforms[side_top ] = transform_t;
transforms[side_left ] = transform_l;
transforms[side_right] = transform_r;
// для отображения разных камер разными цветами
std::map<std::string, Color::Ptr> colors;
colors[side_left ] = std::make_shared<Color>(0, 0, 255);
colors[side_top ] = std::make_shared<Color>(0, 255, 0);
colors[side_right] = std::make_shared<Color>(255, 0, 0);
auto viewer = std::make_unique<Viewer>("Scan3d");
int main_view;
int grid_view;
viewer->initCameraParameters();
viewer->addCoordinateSystem();
// оригинал
viewer->createViewPort(0., 0., 0.5, 1., main_view);
viewer->setBackgroundColor(0.0, 0.0, 0.0, main_view);
// отфильтровано
viewer->createViewPort(0.5, 0.0, 1., 1., grid_view);
viewer->setBackgroundColor(0.2, 0.2, 0.2, grid_view);
pcl::PointXYZ p0, px, py, pz, pc0, pc1;
p0.x = p0.y = p0.z = 0;
px.x = py.y = 2048;
pz.z = 2048 * 3;
// координатные оси
viewer->addLine(p0, px, 1,0,0, "X axis", main_view);
viewer->addLine(p0, py, 0,1,0, "Y axis", main_view);
viewer->addLine(p0, pz, 0,0,1, "Z axis", main_view);
pc0.x = Xcamera;
pc0.y = Zcenter;
pc1 = pc0; pc1.z = pz.z;
// центр вращения
viewer->addLine(pc0, pc1, 0,1,1, "R axis", main_view);
// создать условие обрезки
auto roi = make_roi(Xcamera, Zcenter + 50, 800);
for (const auto & side: sides)
{
std::string file_name;
file_name = base_name + side + ext;
// загрузить
auto orig = loadBuffer(file_name);
// повернуть
auto point = do_transform(*orig, transforms[side]);
colors[side]->setInputCloud(point);
auto color = colors[side];
auto input = std::const_pointer_cast<const Cloud>(point);
// отобразить исходник
viewer->addPointCloud<Point>(input, *color, file_name + "_transform", main_view);
{
auto result = std::make_shared<Cloud>();
// обрезать по ROI
Condition condition;
condition.setCondition(roi);
condition.setInputCloud(input);
condition.setKeepOrganized(true);
condition.filter(*result);
// down sample
Grid grid;
grid.setInputCloud(result);
grid.setLeafSize(20., 20., 20.);
grid.filter(*result);
// удалить выбросы
Statistic stat;
auto out = std::make_shared<Cloud>();
stat.setMeanK(100);
stat.setStddevMulThresh(0.5);
stat.setInputCloud(result);
stat.filter(*out);
// отобразить результат
viewer->addPointCloud<Point>(out,
file_name + "_stat", grid_view);
}
}
while (!viewer->wasStopped())
{
viewer->spinOnce();
}
}
else
{
std::cerr << "Usage: " << argv[0] << " <scan_base>";
}
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment