load scan file from https://github.com/max-dark/scan3d-dataset

buffer.cxx

#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

buffer.hxx

#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

viewer.cxx

#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;
}