FilterKnownObjects.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    VisionX::ArmarXObjects::FilterKnownObjects
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FilterKnownObjects.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 
#include <Eigen/Core>
 
#include <RobotAPI/libraries/core/Pose.h>
 
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr FilterKnownObjects::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def(new visionx::PointCloudProcessorPropertyDefinitions(getConfigIdentifier()));
        def->optional(properties.visualization, "Visualization");
        def->optional(properties.resizeOBBFactor, "ResizeOBBFactor");
        return def;
    }
 
 
    void FilterKnownObjects::onInitPointCloudProcessor()
    {
        filter.setNegative(true);
        // filter.setKeepOrganized(true);
    }
 
 
    void FilterKnownObjects::onConnectPointCloudProcessor()
    {
        enableResultPointClouds<PointT>(getName() + "Result");
        enableResultPointClouds<PointT>(getName() + "RemovedObjects");
        
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
        
    }
 
 
    void FilterKnownObjects::onDisconnectPointCloudProcessor()
    {
    }
 
    void FilterKnownObjects::onExitPointCloudProcessor()
    {
    }
 
 
    void FilterKnownObjects::process()
    {
        // Fetch input point cloud.
        pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
        pcl::PointCloud<PointT>::Ptr removedObjects(new pcl::PointCloud<PointT>());
 
        if (waitForPointClouds())
        {
            getPointClouds(inputCloud);
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error while waiting for point cloud data.";
            return;
        }
 
 
        viz::Layer layer = arviz.layer("OBBB");
        std::vector<std::string> objects_removed;
 
        const std::vector<objpose::ObjectPose> objectPoses = ObjectPoseClient::getObjectPoses();
        for (const objpose::ObjectPose& objectPose : objectPoses)
        {
 
 
            if (!objectPose.oobbGlobal().has_value())
            {
                ARMARX_INFO << "no object bounding box";
                continue;
            }
 
 
            std::string s = objectPose.objectID.str(); 
 
            std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c){
                    return std::tolower(c); });
 
            if(s.find("building") != std::string::npos)
            {
                continue;
            }
 
            if(s.find("interior") != std::string::npos)
            {
                continue;
            }
 
            if(s.find("workbench") != std::string::npos)
            {
                continue;
            }
 
 
            objects_removed.push_back(objectPose.objectID.str());
 
            const Eigen::Matrix4f globalPose = objectPose.objectPoseGlobal;
 
            const simox::OrientedBoxf localOOBB = objectPose.localOOBB.value();
 
            Eigen::Vector3f dim = localOOBB.dimensions();
            dim += Eigen::Vector3f::Ones() * properties.resizeOBBFactor;
 
            Eigen::Vector3f maxPt = dim / 2.0;
            Eigen::Vector3f minPt = -1.0 * maxPt;
 
            Eigen::Matrix4f m = globalPose * localOOBB.transformation_centered();
            Eigen::Affine3f transform;
            transform.matrix() = m;
 
            filter.setTransform(transform.inverse());
            filter.setMin(Eigen::Vector4f(minPt.x(), minPt.y(), minPt.z(), 1.0));
            filter.setMax(Eigen::Vector4f(maxPt.x(), maxPt.y(), maxPt.z(), 1.0));
 
            filter.setInputCloud(inputCloud);
 
            filter.setNegative(false);
            pcl::PointCloud<PointT>::Ptr object(new pcl::PointCloud<PointT>());
            filter.filter(*object);
            (*removedObjects) += (*object);
 
            filter.setNegative(true);
            filter.filter(*inputCloud);
 
            if (properties.visualization)
            {
                viz::Box b("oobb" + s);
                b.pose(m);
                b.size(dim);
                b.color(simox::Color::blue(128, 128));
                layer.add(b);
            }
 
            //const simox::OrientedBoxf globalOOBB = objectPose.oobbGlobal().value();
            //layer.add(viz::Box("box_" + s).set(globalOOBB).color(simox::Color::red(128, 128)));
        }
        ARMARX_INFO << deactivateSpam(5) << "removed " << VAROUT(objects_removed) <<  " from point cloud";
 
        if (properties.visualization)
        {
            arviz.commit(layer);
        }
 
        // Publish result point cloud.
        provideResultPointClouds(inputCloud, getName() + "Result");
        provideResultPointClouds(removedObjects, getName() + "RemovedObjects");
    }
 
 
    void FilterKnownObjects::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the layout.
 
        GridLayout grid;
        int row = 0;
        {
 
            grid.add(tab.extractGrasps, {row, 0}, {2, 1});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
 
    void FilterKnownObjects::RemoteGui_update()
    {
 
    }
 
    std::string FilterKnownObjects::getDefaultName() const
    {
        return GetDefaultName();
    }
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(FilterKnownObjects, FilterKnownObjects::GetDefaultName());
 
 
 
}  // namespace armarx