main.cpp

Go to the documentation of this file.

 
 
#include <cstdlib>
#include <string>
 
#include "ArmarXCore/core/logging/Logging.h"
 
#include "MapRegistration.h"
#include "models.h"
#include "pcl/io/pcd_io.h"
 
namespace x = armarx;
 
int
main()
{
 
    auto mapRegistration = x::MapRegistration::createFromFile(
        "/home/fabi/repos/RobotComponents/data/RobotComponents/maps/2021-04-06-09-27.carto");
    const auto cloud = mapRegistration.readPointClouds();
 
    const auto clusters = mapRegistration.detectClusters(cloud);
 
    int i = 0;
    for (const auto& cluster : clusters)
    {
        pcl::io::savePCDFileASCII("/tmp/cluster_" + std::to_string(i) + ".pcd", *cluster.points);
        i++;
    }
 
 
    const x::Model pillarFront = x::makeCube({-1.8, 3.85 - 0.112}, 0.42);
    const x::Model pillarBack = x::makeCube({-1.8, -0.4}, 0.42);
    const std::vector<x::Model> models{pillarFront, pillarBack};
 
 
    const auto associations = mapRegistration.matchModelsToClusters(models, clusters);
 
 
    std::vector<x::MapRegistration::ModelCorrection> corrections;
 
 
    int j = 0;
    for (const auto& association : associations)
    {
 
        // mapRegistration.align(model, cluster);
        pcl::io::savePCDFileASCII("/tmp/assoc_" + std::to_string(j) + ".pcd",
                                  *association.cluster.points);
 
        x::MapRegistration::ModelCorrection mc =
            mapRegistration.alignModelToCluster(association.model, association.cluster);
        corrections.push_back(mc);
 
        ARMARX_INFO << "Correction" << mc.correction.translation();
 
        j++;
    }
 
    x::MapRegistration::ModelCorrection combinedCorrection =
        mapRegistration.computeCombinedCorrection(models, corrections);
 
    mapRegistration.visualizeResult(cloud, models, combinedCorrection);
 
 
    pcl::io::savePCDFileASCII("/tmp/cloud.pcd", *cloud);
 
    return EXIT_SUCCESS;
}