ArMarkerLocalizer.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-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    VisionX::Component
 * @author     David Schiebener (schiebener at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <opencv2/calib3d.hpp>
 
#include "ArMarkerLocalizer.h"
 
// RobotState
#include <RobotAPI/libraries/core/FramedPose.h>
 
// MemoryX
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
// IVT
#include <Calibration/Calibration.h>
#include <Image/IplImageAdaptor.h>
 
#include <math.h>
 
#include <VisionX/tools/ImageUtil.h>
 
using namespace armarx;
using namespace ::visionx;
using namespace memoryx;
using namespace memoryx::EntityWrappers;
 
 
 
 
ArMarkerLocalizerPropertyDefinitions::ArMarkerLocalizerPropertyDefinitions(std::string prefix) :
    ImageProcessorPropertyDefinitions(prefix)
{
    defineOptionalProperty<float>("MarkerSize", 40.0, "The side length of the marker(s)");
    defineOptionalProperty<std::string>("ReferenceFrameName", "DepthCamera", "Name of the ReferenceFrameName");
    defineOptionalProperty<std::string>("AgentName", "Armar6", "name of the agent");
    defineOptionalProperty<std::string>("ImageProviderName", "ImageProvider", "name of the image provider to use");
}
 
armarx::PropertyDefinitionsPtr ArMarkerLocalizer::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr defs(new ArMarkerLocalizerPropertyDefinitions(getConfigIdentifier()));
 
    defs->optional(visuEnabled, "visu.enabled", "If true, visualize marker position.");
 
    return defs;
}
 
std::string ArMarkerLocalizer::getDefaultName() const
{
    return "ArMarkerLocalizer";
}
 
 
 
void ArMarkerLocalizer::onInitImageProcessor()
{
    imageProviderName = getProperty<std::string>("ImageProviderName").getValue();
}
 
 
void ArMarkerLocalizer::onConnectImageProcessor()
{
    markerSize = getProperty<float>("MarkerSize").getValue();
    ARMARX_VERBOSE << "markerSize: " << markerSize;
 
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(imageProviderName);
    StereoCalibrationInterfacePrx calibrationProvider = StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
    std::unique_ptr<CStereoCalibration> stereoCalibration(visionx::tools::convert(calibrationProvider->getStereoCalibration()));
    CCalibration::CCameraParameters ivtCameraParameters = stereoCalibration->GetLeftCalibration()->GetCameraParameters();
    cv::Mat cameraMatrix(3, 3, cv::DataType<float>::type);
    cameraMatrix.at<float>(0, 0) = ivtCameraParameters.focalLength.x;
    cameraMatrix.at<float>(0, 1) = 0;
    cameraMatrix.at<float>(0, 2) = ivtCameraParameters.principalPoint.x;
    cameraMatrix.at<float>(1, 0) = 0;
    cameraMatrix.at<float>(1, 1) = ivtCameraParameters.focalLength.y;
    cameraMatrix.at<float>(1, 2) = ivtCameraParameters.principalPoint.y;
    cameraMatrix.at<float>(2, 0) = 0;
    cameraMatrix.at<float>(2, 1) = 0;
    cameraMatrix.at<float>(2, 2) = 1;
    ARMARX_VERBOSE << "cameraMatrix: " << cameraMatrix;
    cv::Mat distortionParameters(4, 1, cv::DataType<float>::type);
 
    if (!calibrationProvider->getImagesAreUndistorted())
    {
        distortionParameters.at<float>(0, 0) = ivtCameraParameters.distortion[0];
        distortionParameters.at<float>(1, 0) = ivtCameraParameters.distortion[1];
        distortionParameters.at<float>(2, 0) = ivtCameraParameters.distortion[2];
        distortionParameters.at<float>(3, 0) = ivtCameraParameters.distortion[3];
    }
    else
    {
        distortionParameters.at<float>(0, 0) = distortionParameters.at<float>(1, 0) = distortionParameters.at<float>(2, 0) = distortionParameters.at<float>(3, 0) = 0;
    }
 
    cv::Size imageSize;
    imageSize.width = ivtCameraParameters.width;
    imageSize.height = ivtCameraParameters.height;
    arucoCameraParameters.setParams(cameraMatrix, distortionParameters, imageSize);
    //markerDetector.setCornerRefinementMethod(aruco::MarkerDetector::CornerRefinementMethod::NONE);
 
    startingTime = IceUtil::Time::now();
 
    cameraImages = new CByteImage*[2];
 
    cameraImages[0] = tools::createByteImage(imageProviderInfo);
    cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
    createRemoteGuiTab();
    RemoteGui_startRunningTask();
}
 
void ArMarkerLocalizer::process()
{
    if (!waitForImages(500))
    {
        ARMARX_WARNING << "Timeout or error in wait for images";
        return;
    }
    getImages(cameraImages);
 
    ArMarkerLocalizationResultList result = localizeAllMarkersInternal();
    {
        std::unique_lock lock(resultMutex);
        lastLocalizationResult = result;
    }
    if (visuEnabled)
    {
        viz::Layer layer = arviz.layer("Marker Poses");
 
        for (const auto& r : result)
        {
            layer.add(viz::Pose(std::to_string(r.id))
                      .pose(FramedPosePtr::dynamicCast(r.pose)->toEigen()));
        }
 
        arviz.commit(layer);
    }
}
 
 
 
ArMarkerLocalizationResultList ArMarkerLocalizer::localizeAllMarkersInternal()
{
    IplImage* imageIpl = IplImageAdaptor::Adapt(cameraImages[0]);
    cv::Mat imageOpenCV = cv::cvarrToMat(imageIpl);
 
    ARMARX_VERBOSE << "Calling marker detector";
    std::vector<aruco::Marker> markers;
    markerDetector.detect(imageOpenCV, markers, arucoCameraParameters, markerSize);
 
    ARMARX_INFO << deactivateSpam() << "Localized " << markers.size() << " markers: "
                << ARMARX_STREAM_PRINTER
    {
        for (const auto& m : markers)
        {
            out << m.id << "  ";
        }
    };
 
 
    std::string refFrame = getProperty<std::string>("ReferenceFrameName").getValue();
    const auto agentName = getProperty<std::string>("AgentName").getValue();
 
    visionx::ArMarkerLocalizationResultList resultList;
 
    for (aruco::Marker marker : markers)
    {
        // assemble result
        visionx::ArMarkerLocalizationResult result;
 
        // position and orientation
        marker.calculateExtrinsics(markerSize, arucoCameraParameters.CameraMatrix, arucoCameraParameters.Distorsion);
        Eigen::Vector3f position(marker.Tvec.at<float>(0, 0), marker.Tvec.at<float>(1, 0), marker.Tvec.at<float>(2, 0));
        Eigen::Matrix3f orientation;
 
 
        cv::Matx33f cvOrientation;
 
        cv::Rodrigues(marker.Rvec, cvOrientation);
 
        // putting z column first, fixes wrong orientation.
        orientation << cvOrientation(0, 2), cvOrientation(0, 0), cvOrientation(0, 1),
                    cvOrientation(1, 2), cvOrientation(1, 0), cvOrientation(1, 1),
                    cvOrientation(2, 2), cvOrientation(2, 0), cvOrientation(2, 1);
 
 
        //result.position = new armarx::FramedPosition(position, refFrame, agentName);
        //result.orientation = new armarx::FramedOrientation(orientation, refFrame, agentName);
        result.pose = new armarx::FramedPose(orientation, position, refFrame, agentName);
        result.id = marker.id;
 
        /*Eigen::Vector3f mean;
        mean << 0, 0, 0;
        Eigen::Matrix3f vars;
        vars << 0.1, 0, 0,   0, 0.1, 0,   0, 0, 0.1;
        result.positionNoise = memoryx::MultivariateNormalDistributionPtr(new memoryx::MultivariateNormalDistribution(mean, vars));
 
        // calculate recognition certainty
        result.recognitionCertainty = 1.0;
        result.objectClassName = "Marker_" + std::to_string(marker.id);*/
 
        resultList.push_back(result);
    }
 
    return resultList;
}
 
 
 
visionx::ArMarkerLocalizationResultList visionx::ArMarkerLocalizer::LocalizeAllMarkersNow(const Ice::Current&)
{
    if (!waitForImages(500))
    {
        ARMARX_WARNING << "No new Images available!";
        return visionx::ArMarkerLocalizationResultList();
    }
    getImages(cameraImages);
    return localizeAllMarkersInternal();
}
 
ArMarkerLocalizationResultList ArMarkerLocalizer::GetLatestLocalizationResult(const Ice::Current&)
{
    std::unique_lock lock(resultMutex);
    return lastLocalizationResult;
}
 
 
namespace visionx
{
 
    void ArMarkerLocalizer::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        GridLayout grid;
        int row = 0;
 
        tab.markerSize.setRange(1.0, 1000.0);
        tab.markerSize.setSteps(1000);
        tab.markerSize.setDecimals(1);
        tab.markerSize.setValue(this->markerSize);
 
        grid.add(Label("Marker size:"), {row, 0}).add(tab.markerSize, {row, 1});
        row++;
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void ArMarkerLocalizer::RemoteGui_update()
    {
        if (tab.markerSize.hasValueChanged())
        {
            this->markerSize = tab.markerSize.getValue();
        }
    }
 
}