UCLObjectRecognition.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::UCLObjectRecognition
 * @author     Mirko Waechter (waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "UCLObjectRecognition.h"
#include <Image/PrimitivesDrawerCV.h>
#include <Image/PrimitivesDrawer.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <MemoryX/core/memory/PersistentEntitySegment.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
 
#include <Eigen/Geometry>
#include <Calibration/Calibration.h>
 
using namespace armarx;
 
using namespace std::filesystem;
namespace visionx
{
    //using namespace boost::python;
 
    //void visionx::UCLObjectRecognition::onInitObjectLocalizerProcessor()
    void visionx::UCLObjectRecognition::onInitComponent()
    {
        usingProxy("ObjectPoseProvider");
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    }
 
    //void visionx::UCLObjectRecognition::onConnectObjectLocalizerProcessor()
    void visionx::UCLObjectRecognition::onConnectComponent()
    {
        getProxy(prx, "ObjectPoseProvider");
        debugDrawerTopicPrx = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
        if (!debugDrawerTopicPrx)
        {
            ARMARX_ERROR << "Failed to obtain debug drawer proxy";
            return;
        }
        robotStateComponent = getProxy<armarx::RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
        localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
 
 
    }
 
 
 
 
 
    armarx::PropertyDefinitionsPtr UCLObjectRecognition::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new UCLObjectRecognitionPropertyDefinitions(
                getConfigIdentifier()));
    }
 
 
 
    memoryx::ObjectLocalizationResultList visionx::UCLObjectRecognition::localizeObjectClasses(const memoryx::ObjectClassNameList& classes, const Ice::Current&)
    {
        // static std::size_t flip = 0;
        // static std::size_t flipn = 100;
 
        ARMARX_VERBOSE << /*deactivateSpam(10) <<*/ "Localizing: " << classes;
        auto agentName = getProperty<std::string>("AgentName").getValue();
        auto cameraFrame = getProperty<std::string>("CameraFrameName").getValue();
        auto mapping = armarx::Split(getProperty<std::string>("ObjectNameIdMap"), ";", true, true);
        std::map<std::string, int> nameIdMap;
        for (auto entry : mapping)
        {
            auto pair = armarx::Split(entry, ":");
            ARMARX_CHECK_EQUAL(pair.size(), 2) << pair;
            nameIdMap[pair.at(0)] = armarx::toFloat(pair.at(1));
 
 
            //        debugDrawerTopicPrx->removePoseVisu(getName(), pair.at(0) + "_raw");
            //        debugDrawerTopicPrx->removePoseVisu(getName(), pair.at(0) + "_global");
        }
        memoryx::ObjectLocalizationResultList result;
        for (auto className : classes)
        {
            ARMARX_CHECK_EXPRESSION(nameIdMap.count(className)) << VAROUT(className) << " " << VAROUT(nameIdMap);
            auto id = nameIdMap[className];
            auto regResult = prx->getObjectPose(id);
            if (regResult.confidence <= 0)
            {
                ARMARX_INFO << deactivateSpam(5, className) << "Nothing found for " << className;
                continue;
            }
            memoryx::ObjectLocalizationResult r;
            r.instanceName = className;
            r.objectClassName = className;
            r.recognitionCertainty = regResult.confidence * 0.2;
            ARMARX_CHECK_EQUAL(regResult.matrix4x4.size(), 16) << regResult.confidence;
            Eigen::Matrix4f pose = Eigen::Map<Eigen::Matrix4f>(regResult.matrix4x4.data()).transpose();
            pose.block<3, 1>(0, 3) *= 1000.0;
            ARMARX_INFO << "pose\n" << pose << '\n' << VAROUT(r.recognitionCertainty);
 
            {
                //            debugDrawerTopicPrx->setPoseVisu(getName(), className + "_raw", new Pose {pose});
                RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
                // const Eigen::Matrix4f gcam = localRobot->getRobotNode(cameraFrame)->getGlobalPose();
                const Eigen::Matrix4f gpose = localRobot->getRobotNode(cameraFrame)->getGlobalPose() * pose;
                debugDrawerTopicPrx->setPoseVisu(getName(), className + "_global", new Pose {gpose});
                //            ARMARX_IMPORTANT << "drawing\n" << pose
                //                             << "\ncam\n" << localRobot->getRobotNode(cameraFrame)->getGlobalPose()
                //                             << "\ngp\n" << gpose;
 
 
                //            Eigen::Matrix4f manualcam = gcam;
                //            manualcam(2, 3) += 1000;
                //            Eigen::Matrix4f manualPose = pose;
                //            manualPose.topRightCorner<3, 1>() += manualcam.topRightCorner<3, 1>();
 
                //            //manualPose(2, 3) += 1000;
 
                //            const Eigen::Vector3f gcammanpos = manualcam.topRightCorner<3, 1>();
                //            const Eigen::Vector3f gobjmanpos = manualPose.topRightCorner<3, 1>();
                //            const Eigen::Vector3f gcammanposX = (manualcam * Eigen::Vector4f {1000, 0, 0, 1}).topRightCorner<3, 1>();
                //            const Eigen::Vector3f gcammanposY = (manualcam * Eigen::Vector4f {0, 1000, 0, 1}).topRightCorner<3, 1>();
                //            const Eigen::Vector3f gcammanposZ = (manualcam * Eigen::Vector4f {0, 0, 1000, 1}).topRightCorner<3, 1>();
                //            ARMARX_INFO << VAROUT(manualPose);
                //            debugDrawerTopicPrx->setPoseVisu(getName(), className + "_manualPose", new Pose {manualPose});
                //            debugDrawerTopicPrx->setPoseVisu(getName(), className + "_manualcam", new Pose {manualcam});
 
                //            debugDrawerTopicPrx->setLineVisu(getName(), className + "_cam_x", new Vector3 {gcammanpos}, new Vector3 {gcammanposX}, 5, {1, 0, 0, 1});
                //            debugDrawerTopicPrx->setLineVisu(getName(), className + "_cam_y", new Vector3 {gcammanpos}, new Vector3 {gcammanposY}, 5, {0, 1, 0, 1});
                //            debugDrawerTopicPrx->setLineVisu(getName(), className + "_cam_z", new Vector3 {gcammanpos}, new Vector3 {gcammanposZ}, 5, {0, 0, 1, 1});
                //            debugDrawerTopicPrx->setLineVisu(getName(), className + "_cam_obj", new Vector3 {gcammanpos}, new Vector3 {gobjmanpos}, 5, {0, 0, 0, 1});
            }
 
 
            r.orientation = new FramedOrientation(pose, cameraFrame, agentName);
            r.position = new FramedPosition(pose, cameraFrame, agentName);
            Eigen::Vector3f cov(10, 10, 10000);
            r.positionNoise = new memoryx::MultivariateNormalDistribution(Eigen::Vector3f::Zero(), cov.asDiagonal());
            ARMARX_INFO << "Pose is " << (TimeUtil::GetTime() - IceUtil::Time::milliSeconds(regResult.timestamp)).toMilliSeconds() << " ms old";
            r.timeStamp = new armarx::TimestampVariant(regResult.timestamp * 1000); // ToDo: get timestamp from image
            result.push_back(r);
        }
 
 
        return result;
    }
}