utils.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::armem_objects
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "utils.h"
 
#include <cstddef>
#include <cstdint>
#include <random>
#include <string>
#include <vector>
 
#include <pcl/point_types.h>
 
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <VirtualRobot/ManipulationObject.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/Visualization/VisualizationNode.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h"
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h> // IWYU pragma: keep
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
 
namespace armarx::armem
{
 
    // Samples points from the surface of a TriMeshModel. The sampling is non-uniform
    // and approximates picking barycentric coordinates per face with random weights.
    // Returns a point cloud in the same coordinate frame as the mesh vertices.
    pcl::PointCloud<pcl::PointXYZRGBA>
    samplePointCloudFromTriMesh(const VirtualRobot::TriMeshModel& mesh, int sampleCount)
    {
        pcl::PointCloud<pcl::PointXYZRGBA> cloud;
        if (mesh.faces.empty())
            return cloud;
 
        std::mt19937 gen{std::random_device{}()};
        std::uniform_int_distribution<std::size_t> d{0, mesh.faces.size() - 1};
 
        cloud.points.reserve(static_cast<std::size_t>(sampleCount));
 
        for (int i = 0; i < sampleCount; ++i)
        {
            const auto& f = mesh.faces.at(d(gen));
            const float f0 = 1 + d(gen);
            const float f1 = 1 + d(gen);
            const float f2 = 1 + d(gen);
            Eigen::Vector3f factors = Eigen::Vector3f{f0, f1, f2}.normalized();
            factors /= factors.sum();
            const Eigen::Vector3f pt = factors(0) * mesh.vertices.at(f.id1) +
                                       factors(1) * mesh.vertices.at(f.id2) +
                                       factors(2) * mesh.vertices.at(f.id3);
 
            pcl::PointXYZRGBA point;
            point.x = pt.x();
            point.y = pt.y();
            point.z = pt.z();
 
            // use the color of the first vertex (approximation)
            const auto color = mesh.colors.at(f.id1);
 
            point.r = static_cast<std::uint8_t>(color.r * 255);
            point.g = static_cast<std::uint8_t>(color.g * 255);
            point.b = static_cast<std::uint8_t>(color.b * 255);
 
            cloud.points.push_back(point);
        }
 
        cloud.is_dense = true;
        cloud.width = cloud.points.size();
        cloud.height = 1;
 
        return cloud;
    }
 
    MemoryID
    objects::reconstructObjectInstanceID(const objpose::ObjectPose& objectPose)
    {
        armem::MemoryID id =
            armem::objects::instaceSegmentID.withProviderSegmentName(objectPose.providerName)
                .withEntityName(objectPose.objectID.str())
                .withTimestamp(objectPose.timestamp)
                .withInstanceIndex(0);
        return id;
    }
 
    armarx::armem::arondto::FamiliarObjectInstance
    objects::createFamiliarObjectInstanceFromManipulationObject(
        const VirtualRobot::ManipulationObject& manipulationObject,
        const armarx::ObjectID& objectID)
    {
 
        const std::string agent = ""; // unused as it is the global frame
 
        armarx::armem::arondto::FamiliarObjectInstance familiarObject;
 
        familiarObject.timestamp = armarx::Clock::Now();
 
        // Use the object's global pose as both sensor and global pose. Caller may override frames.
        const Eigen::Matrix4f objPose = manipulationObject.getGlobalPose();
 
        familiarObject.poseSensFrame.pose = objPose;
        familiarObject.poseSensFrame.header.frame = armarx::GlobalFrame;
        familiarObject.poseSensFrame.header.agent = agent;
 
        familiarObject.poseGlobal.emplace();
        familiarObject.poseGlobal->pose = objPose;
        familiarObject.poseGlobal->header.frame = armarx::GlobalFrame;
        familiarObject.poseGlobal->header.agent = agent;
 
        toAron(familiarObject.objectID, objectID);
        familiarObject.confidence = 1.0f;
 
        // Try to obtain the visualization mesh and sample points similarly to the existing example.
        ARMARX_CHECK_NOT_NULL(manipulationObject.getVisualization())
            << "ManipulationObject has no visualization node.";
        const auto mesh = manipulationObject.getVisualization()->getTriMeshModel();
 
        if (mesh && !mesh->faces.empty())
        {
            std::mt19937 gen{std::random_device{}()};
            std::uniform_int_distribution<std::size_t> d{0, mesh->faces.size() - 1};
 
            ARMARX_CHECK_NOT_EMPTY(mesh->faces);
 
            auto nonUniformSampleSurface = [mesh = *mesh, &gen, &d]() -> pcl::PointXYZRGBA
            {
                const auto& f = mesh.faces.at(d(gen));
                const float f0 = 1 + d(gen);
                const float f1 = 1 + d(gen);
                const float f2 = 1 + d(gen);
                Eigen::Vector3f factors = Eigen::Vector3f{f0, f1, f2}.normalized();
                factors /= factors.sum();
                const Eigen::Vector3f pt = factors(0) * mesh.vertices.at(f.id1) +
                                           factors(1) * mesh.vertices.at(f.id2) +
                                           factors(2) * mesh.vertices.at(f.id3);
 
                pcl::PointXYZRGBA point;
                point.x = pt.x();
                point.y = pt.y();
                point.z = pt.z();
 
                // we use the color of the first vertex (approximation)
                const auto color = mesh.colors.at(f.id1);
 
                point.r = static_cast<std::uint8_t>(color.r * 255);
                point.g = static_cast<std::uint8_t>(color.g * 255);
                point.b = static_cast<std::uint8_t>(color.b * 255);
 
                return point;
            };
 
            const int sampleCount = 1000;
            for (int i = 0; i < sampleCount; ++i)
            {
                familiarObject.points.points.push_back(nonUniformSampleSurface());
            }
 
            familiarObject.points.header.frame_id = armarx::GlobalFrame;
            familiarObject.points.is_dense = true;
            familiarObject.points.width = familiarObject.points.points.size();
            familiarObject.points.height = 1;
 
            std::vector<Eigen::Vector3f> sampledPoints;
            sampledPoints.reserve(familiarObject.points.points.size());
            for (const auto& pt : familiarObject.points.points)
            {
                sampledPoints.emplace_back(pt.x, pt.y, pt.z);
            }
 
            auto aabb = simox::aabb::from_points(sampledPoints);
            familiarObject.bounding_box.center = aabb.center();
            familiarObject.bounding_box.extents = aabb.extents();
        }
 
        return familiarObject;
    }
} // namespace armarx::armem