Point cloud preprocessing

CMakeLists.txt

cmake_minimum_required(VERSION 3.20.0)
project(pcl-import VERSION 0.1.0 LANGUAGES C CXX)

set(CMAKE_CXX_STANDARD 17)

set(CCCORELIB_SHARED OFF)
add_subdirectory(extern/CCCoreLib EXCLUDE_FROM_ALL)

add_executable(pcl-import main.cpp)
target_link_libraries(pcl-import CCCoreLib)

main.cpp

#include <iostream>
#include <map>
#include <vector>
#include "extern/CCCoreLib/include/ScalarFieldTools.h"
#include "extern/CCCoreLib/include/PointCloud.h"
#include "extern/CCCoreLib/include/ReferenceCloud.h"
#include "extern/CCCoreLib/include/DistanceComputationTools.h"
#include "extern/csv-parser/single_include/csv.hpp"

struct reference_point {
    CCVector3 position;
    int rssi;

    reference_point(CCVector3 position, int rssi) :
        position(position), rssi(rssi)
    {}
};

static void save_point_cloud(std::ostream& out, CCCoreLib::PointCloud& pc)
{
    out << "VERSION 0.7\n";

    out << "FIELDS x y z";
    for (unsigned int i = 0; i < pc.getNumberOfScalarFields(); i++)
        out << " " << pc.getScalarField(i)->getName();
    out << "\n";

    out << "SIZE 4 4 4";
    for (unsigned int i = 0; i < pc.getNumberOfScalarFields(); i++)
        out << " 4";
    out << "\n";

    out << "TYPE F F F";
    for (unsigned int i = 0; i < pc.getNumberOfScalarFields(); i++)
        out << " F";
    out << "\n";

    out << "WIDTH " << pc.size() << "\n"
        << "HEIGHT 1\n"
        << "POINTS " << pc.size() << "\n"
        << "DATA ascii\n";

    pc.setCurrentScalarField(pc.getScalarFieldIndexByName("rssi.smoothed"));

    for (unsigned int i = 0; i < pc.size(); i++) {
        auto point = pc.getPoint(i);
        out << point->x << " " << point->y << " " << point->z;
        for (unsigned int j = 0; j < pc.getNumberOfScalarFields(); j++) {
            auto scalar = pc.getScalarField(j)->getValue(i);
            out << " " << scalar;
        }
        out << "\n";
    }
}

#define REF_RSSI (-59)
#define ABSORPTION 4.57

int main(int argc, char** argv)
{
    csv::CSVReader nodesReader("../uwb/nodes.csv");
    std::map<std::string, CCVector3> node_locations;
    for (csv::CSVRow const& row: nodesReader) {
        node_locations[row["node"].get<std::string>()] = CCVector3 {
            row["X"].get<float>(),
            row["Y"].get<float>(),
            row["Z"].get<float>()
        };
    }
    
    csv::CSVReader pointsReader("../uwb/uwb.csv");
    std::map<std::string, std::vector<reference_point>> points_by_node;
    for (csv::CSVRow const& row: pointsReader) {
        points_by_node[row["node"].get<std::string>()].emplace_back(
            CCVector3 {
                row["X"].get<float>(),
                row["Y"].get<float>(),
                row["Z"].get<float>()
            },
            row["rssi"].get<int>()
        );
    }

    std::map<std::string, std::shared_ptr<CCCoreLib::PointCloud>> clouds_by_node;

    for (auto& [node, points] : points_by_node) {
        auto pc = std::make_shared<CCCoreLib::PointCloud>();
        auto raw_scalar_field_index = pc->addScalarField("rssi");
        pc->setCurrentScalarField(raw_scalar_field_index);

        auto node_location = node_locations.at(node);

        pc->reserve(points.size());
        for (auto const& point : points) {
            pc->addPoint(point.position);
            auto measured_distance = std::pow(10, float(REF_RSSI - point.rssi) / (10.0f * ABSORPTION));
            pc->addPointScalarValue(measured_distance);
        }

        points.clear();

        auto scalar_field_index = pc->addScalarField("rssi.smoothed");
        pc->setCurrentInScalarField(scalar_field_index);

        // 0.35, -1
        if (!CCCoreLib::ScalarFieldTools::applyScalarFieldGaussianFilter(0.3, pc.get(), -1)) {
            std::cerr << "Failed to build rssi.smoothed scalar field.\n";
            return 1;
        }

        //pc->setCurrentScalarField(scalar_field_index);
        pc->setCurrentScalarField(raw_scalar_field_index);

        for (unsigned int i = 0; i < pc->size(); i++) {
            auto measured_distance = pc->getPointScalarValue(i);
            //auto expected_distance = (*pc->getPoint(i) - node_location).norm();
            auto rssi = REF_RSSI - 10 * ABSORPTION * std::log10(measured_distance);
            pc->setPointScalarValue(i, rssi);
        }

        clouds_by_node[node] = pc;
    }

    auto cloud_with_most_points = std::max_element(clouds_by_node.begin(), clouds_by_node.end(), [](auto& p1, auto& p2) { return p1.second->size() < p2.second->size(); })->second;
    auto output_cloud = *cloud_with_most_points;
    output_cloud.deleteAllScalarFields();

    for (auto& [node, cloud] : clouds_by_node) {
        CCCoreLib::ReferenceCloud closest_points { &output_cloud };
        CCCoreLib::DistanceComputationTools::Cloud2CloudDistancesComputationParams params;
        params.CPSet = &closest_points;
        auto temp_scalar_index = output_cloud.addScalarField("temp");
        output_cloud.setCurrentScalarField(temp_scalar_index);
        if (CCCoreLib::DistanceComputationTools::computeCloud2CloudDistances(&output_cloud, cloud.get(), params) < 0) {
            std::cerr << "Failed to build closest point set.\n";
            return 1;
        }
        output_cloud.deleteScalarField(temp_scalar_index);

        std::stringstream sfNameStream;
        sfNameStream << "rssi." << node;
        std::string sfName = sfNameStream.str();
        unsigned int in_scalar_field = output_cloud.addScalarField(sfName.c_str());
        output_cloud.setCurrentInScalarField(in_scalar_field);

        for (unsigned int i = 0; i < closest_points.size(); i++) {
            auto rssi = cloud->getPointScalarValue(closest_points.getPointGlobalIndex(i));
            output_cloud.setPointScalarValue(i, rssi);
        }
        std::cout << "closest points: " << closest_points.size() << "\n";
    }

    std::ofstream fp("../uwb/combined.pcd");
    save_point_cloud(fp, output_cloud);
}