ObjectLocalizerDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarXSimulation::ArmarXObjects::ObjectLocalizerDynamicSimulation
 * @author     welke ( welke at kit dot edu )
 * @date       2013
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectLocalizerDynamicSimulation.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
// EarlyVision
//#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
//#include <MemoryX/libraries/helpers/EarlyVisionHelpers/UnscentedTransform.h>
 
 
using namespace armarx;
 
ObjectLocalizerDynamicSimulationPropertyDefinitions::
    ObjectLocalizerDynamicSimulationPropertyDefinitions(std::string prefix) :
    ComponentPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>(
        "SimulatorProxyName", "Simulator", "name of dynamics simulator proxy");
    defineOptionalProperty<std::string>(
        "RobotName", "Armar3", "Name of robot used for calculating reference frame");
    defineOptionalProperty<std::string>(
        "ReferenceFrameName", "EyeLeftCamera", "Name of reference frame to use for pose");
    //defineRequiredProperty<std::string>("CalibrationFile", "Camera calibration file, will be used for uncertainty calculation");
    //defineOptionalProperty<float>("2DLocalizationNoise", 4.0, "2D localization noise of object recognition. Used in order to calculate the 3D world localization noise.");
    defineOptionalProperty<float>(
        "RecognitionCertainty", 0.9f, "Certainty of recognition used in simulation (0...1).");
    defineOptionalProperty<bool>(
        "VisibilityCheck",
        false,
        "Whether to simulate camera and checking visibility within camera");
}
 
void
ObjectLocalizerDynamicSimulation::onInitComponent()
{
    // using dynamic simulation
    usingProxy(getProperty<std::string>("SimulatorProxyName").getValue());
 
    robotName = getProperty<std::string>("RobotName").getValue();
    frameName = getProperty<std::string>("ReferenceFrameName").getValue();
 
    recognitionCertainty = getProperty<float>("RecognitionCertainty").getValue();
    performVisibilityCheck = getProperty<bool>("VisibilityCheck").getValue();
 
    if (performVisibilityCheck)
    {
        ARMARX_WARNING << "Visibility check is nyi...";
    }
 
 
    // stereo calibration
    /*std::string calibrationFileName = getProperty<std::string>("CalibrationFile") .getValue();
    std::string fullCalibrationFileName;
 
    if(!ArmarXDataPath::getAbsolutePath(calibrationFileName, fullCalibrationFileName))
    {
        ARMARX_ERROR << "Could not find camera calibration file in ArmarXDataPath: " << calibrationFileName;
    }
 
    if (!stereoCalibration.LoadCameraParameters(
            fullCalibrationFileName.c_str(), true))
    {
        ARMARX_ERROR << "Error loading camera calibration file: " << fullCalibrationFileName;
    }
 
    // setup noise
    imageNoiseLeft(0) = imageNoiseLeft(1) = getProperty<float>("2DLocalizationNoise").getValue();
    imageNoiseRight(0) = imageNoiseRight(1) = getProperty<float>("2DLocalizationNoise").getValue();
    */
 
    ARMARX_VERBOSE << "Simulating object localization of type " << getName() << " for robot "
                   << robotName;
}
 
void
ObjectLocalizerDynamicSimulation::onConnectComponent()
{
    simulatorPrx =
        getProxy<SimulatorInterfacePrx>(getProperty<std::string>("SimulatorProxyName").getValue());
}
 
::memoryx::MultivariateNormalDistributionPtr
ObjectLocalizerDynamicSimulation::computePositionNoise(const Eigen::Vector3f& pos)
{
    memoryx::FloatVector mean;
    mean.push_back(pos(0));
    mean.push_back(pos(1));
    mean.push_back(pos(2));
 
    // assuming 2 mm noise
    memoryx::MultivariateNormalDistributionPtr posDist =
        new memoryx::MultivariateNormalDistribution(3);
    posDist->setMean(mean);
    posDist->setCovariance(0, 0, 2.0f);
    posDist->setCovariance(1, 1, 2.0f);
    posDist->setCovariance(2, 2, 2.0f);
    return posDist;
}
 
memoryx::ObjectLocalizationResultList
ObjectLocalizerDynamicSimulation::localizeObjectClasses(
    const memoryx::ObjectClassNameList& objectClassNames,
    const Ice::Current&)
{
    //    ARMARX_VERBOSE << "Entering localizeObjectClasses" << flush;
    memoryx::ObjectLocalizationResultList resultList;
 
    // assure is fully connected
    getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
 
    // go through requested object classes
    for (const auto& className : objectClassNames)
    {
        // query simulator for class
        ARMARX_INFO << deactivateSpam(5) << robotName << " " << frameName << " " << className
                    << flush;
        ObjectClassInformationSequence objectInstances =
            simulatorPrx->getObjectClassPoses(robotName, frameName, className);
 
        // go through instances
        for (const auto& instanceInfo : objectInstances)
        {
            memoryx::ObjectLocalizationResult result;
 
            result.timeStamp = new TimestampVariant(instanceInfo.timestampMicroSeconds);
 
            // class name
            result.objectClassName = instanceInfo.className;
 
            // pose
            armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(instanceInfo.pose);
            ARMARX_INFO << deactivateSpam(5) << className << " pose: " << pose->output();
            FramedPositionPtr position = pose->getPosition();
            result.position = position;
            result.orientation = pose->getOrientation();
 
            // uncertainties
            result.positionNoise = computePositionNoise(position->toEigen());
            result.recognitionCertainty = recognitionCertainty;
 
            resultList.push_back(result);
 
            /*
                        if(performVisibilityCheck)
                        {
                            if(isVisible(armarx::FramedPositionPtr::dynamicCast(result.position)->toEigen()))
                                resultList.push_back(result);
                        } else
                            resultList.push_back(result);
            */
        }
    }
 
 
    ARMARX_VERBOSE << deactivateSpam(5) << "Found " << resultList.size() << " instances";
 
    TimeUtil::SleepMS(50);
 
    return resultList;
}
 
std::string
ObjectLocalizerDynamicSimulation::getDefaultName() const
{
    return "ObjectLocalizerDynamicSimulation";
}
 
/*
memoryx::MultivariateNormalDistributionPtr ObjectLocalizerDynamicSimulation::calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point)
{
    // initialize noise
    UnscentedTransform ut;
 
    Eigen::MatrixXd combinedNoise(4,4);
    combinedNoise.setZero();
    combinedNoise(0,0) = imageNoiseLeft(0);
    combinedNoise(1,1) = imageNoiseLeft(1);
    combinedNoise(2,2) = imageNoiseRight(0);
    combinedNoise(3,3) = imageNoiseRight(1);
 
    Eigen::VectorXd mean(4);
    mean << left_point.x, left_point.y, right_point.x, right_point.y;
 
    Gaussian imageSpaceNoise(4);
    imageSpaceNoise.setCovariance(combinedNoise);
    imageSpaceNoise.setMean(mean);
 
    // calculate sigma points
    Eigen::MatrixXd sigmapoints = ut.getSigmaPoints(imageSpaceNoise);
 
    // pass sigma points through system
    Vec2d l,r;
    Vec3d w;
    Eigen::VectorXd base(4);
    Eigen::VectorXd world(4);
 
    Eigen::MatrixXd processedpoints(3,sigmapoints.cols());
 
    for(int n = 0 ; n < sigmapoints.cols() ; n++)
    {
 
        Math2d::SetVec(l, sigmapoints(0, n), sigmapoints(1, n));
        Math2d::SetVec(r, sigmapoints(2, n), sigmapoints(3, n));
 
        // calc 3d point (2D points are rectified but not distorted)
        stereoCalibration.Calculate3DPoint(l, r, w, true, true);
        world << w.x, w.y, w.z, 1.0f;
 
        processedpoints(0, n) = world(0);
        processedpoints(1, n) = world(1);
        processedpoints(2, n) = world(2);
    }
 
    // recover covariance
    Gaussian worldSpaceNoise = ut.extractGaussian(processedpoints);
 
    return memoryx::EarlyVisionConverters::convertToMemoryX_MULTI(worldSpaceNoise);
    }
 
memoryx::MultivariateNormalDistributionPtr ObjectLocalizerDynamicSimulation::calculateLocalizationUncertainty(const Eigen::Vector3f& position)
{
    // calculate 2d points for localization uncerainty
    Vec2d left2d, right2d;
    getImagePositions(position, left2d, right2d);
 
    return calculateLocalizationUncertainty(left2d, right2d);
}
 
bool ObjectLocalizerDynamicSimulation::isVisible(const Eigen::Vector3f& position)
{
    // calculate 2d points for localization uncerainty
    Vec2d left2d, right2d;
    getImagePositions(position, left2d, right2d);
 
    // check if within image
    const CCalibration::CCameraParameters& leftParams = stereoCalibration.GetLeftCalibration()->GetCameraParameters();
    const CCalibration::CCameraParameters& rightParams = stereoCalibration.GetRightCalibration()->GetCameraParameters();
 
    bool visibleInLeft = true;
    if( (left2d.x < 0) || (left2d.x >= leftParams.width) )
        visibleInLeft = false;
    if( (left2d.y < 0) || (left2d.y >= leftParams.height) )
        visibleInLeft = false;
 
    bool visibleInRight = true;
    if( (right2d.x < 0) || (right2d.x >= rightParams.width) )
        visibleInRight = false;
    if( (right2d.y < 0) || (right2d.y >= rightParams.height) )
        visibleInRight = false;
 
    ARMARX_INFO << "ObjectLocalizer: " << visibleInRight << " " << visibleInLeft << flush;
 
    return visibleInLeft && visibleInRight;
}
 
void ObjectLocalizerDynamicSimulation::getImagePositions(const Eigen::Vector3f& position, Vec2d& left_point, Vec2d& right_point)
{
    Vec2d left2d, right2d;
    Vec3d pos;
    Math3d::SetVec(pos, position(0), position(1), position(2));
 
    stereoCalibration.GetLeftCalibration()->WorldToImageCoordinates(pos, left2d);
    stereoCalibration.GetRightCalibration()->WorldToImageCoordinates(pos, right2d);
 
    // transform to rectified coordinates
    Mat3d inverseHLeft, inverseHRight;
    Math3d::Invert(stereoCalibration.rectificationHomographyLeft, inverseHLeft);
    Math3d::Invert(stereoCalibration.rectificationHomographyRight, inverseHRight);
    Math2d::ApplyHomography(inverseHLeft, left2d, left_point);
    Math2d::ApplyHomography(inverseHRight, right2d, right_point);
}*/
 
 
PropertyDefinitionsPtr
ObjectLocalizerDynamicSimulation::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new ObjectLocalizerDynamicSimulationPropertyDefinitions(getConfigIdentifier()));
}