ArMarkerLocalizerOpenCV.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 "ArMarkerLocalizerOpenCV.h"
 
#include <Eigen/Core>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
 
// IVT
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/OpenCVUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/IplImageAdaptor.h>
 
namespace visionx
{
 
    ArMarkerLocalizerOpenCVPropertyDefinitions::ArMarkerLocalizerOpenCVPropertyDefinitions(
        std::string prefix) :
        ImageProcessorPropertyDefinitions(prefix)
    {
    }
 
    armarx::PropertyDefinitionsPtr
    ArMarkerLocalizerOpenCV::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new ArMarkerLocalizerOpenCVPropertyDefinitions(getConfigIdentifier()));
 
        defs->optional(
            p.imageProviderName, "img.ImageProviderName", "Name of the image provider to use.");
        defs->optional(
            p.referenceFrame, "img.ReferenceFrameName", "Name of the ReferenceFrameName");
        defs->optional(p.agentName, "img.AgentName", "Name of the agent");
        defs->optional(p.cameraIndex, "img.cameraIndex", "Camera index. Left = 0, Right = 1, ...");
 
        std::string pattern = "[ k3 [, k4, k5, k6 [, s1, s2, s3, s4 ] ] ]";
        // defs->optional(p.extraDistortionCoeffs, "cam.ExtraDistortionCoeffs",
        defs->defineOptionalProperty<std::vector<std::string>>(
                "cam.ExtraDistortionCoeffs",
                {},
                "Optional extra distortion coefficients (which cannot be retrieved from the image "
                "provider)."
                "\nThe expected parameters are: " +
                    pattern +
                    ""
                    "\nThat is, you must specifiy 1 (k3), 4 (k3-k6), or 8 (k3-k6, s1-s4) elements."
                    "\nNote: k1, k2, p1, p2 are retrieved from the image provider.")
            .map(pattern, {});
 
        defs->defineOptionalProperty<float>(
            "ar.MarkerSize", p.markerSize.load(), "The side length of the marker(s)");
        defs->optional(p.dictionary, "ar.Dictionary", "The ArUco dictionary.")
            .map("4X4_50", cv::aruco::DICT_4X4_50)
            .map("4X4_100", cv::aruco::DICT_4X4_100)
            .map("4X4_250", cv::aruco::DICT_4X4_250)
            .map("4X4_1000", cv::aruco::DICT_4X4_1000)
            .map("5X5_50", cv::aruco::DICT_5X5_50)
            .map("5X5_100", cv::aruco::DICT_5X5_100)
            .map("5X5_250", cv::aruco::DICT_5X5_250)
            .map("5X5_1000", cv::aruco::DICT_5X5_1000)
            .map("6X6_50", cv::aruco::DICT_6X6_50)
            .map("6X6_100", cv::aruco::DICT_6X6_100)
            .map("6X6_250", cv::aruco::DICT_6X6_250)
            .map("6X6_1000", cv::aruco::DICT_6X6_1000)
            .map("7X7_50", cv::aruco::DICT_7X7_50)
            .map("7X7_100", cv::aruco::DICT_7X7_100)
            .map("7X7_250", cv::aruco::DICT_7X7_250)
            .map("7X7_1000", cv::aruco::DICT_7X7_1000)
            .map("ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL);
 
        defs->defineOptionalProperty<std::string>("ar.MarkerGeneration",
                                                  "https://chev.me/arucogen/",
                                                  "You can generate ArUco markers here.");
 
        defs->defineOptionalProperty<bool>(
            "visu.Enabled", p.visuEnabled.load(), "If true, visualize marker poses.");
 
        return defs;
    }
 
    ArMarkerLocalizerOpenCV::ArMarkerLocalizerOpenCV()
    {
        addPlugin(virtualRobotReaderPlugin);
    }
 
    std::string
    ArMarkerLocalizerOpenCV::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    ArMarkerLocalizerOpenCV::GetDefaultName()
    {
        return "ArMarkerLocalizerOpenCV";
    }
 
    void
    ArMarkerLocalizerOpenCV::onInitImageProcessor()
    {
        ARMARX_INFO << VAROUT(p.dictionary);
        *arucoDictionary = cv::aruco::getPredefinedDictionary(p.dictionary);
        p.markerSize = getProperty<float>("ar.MarkerSize").getValue();
        ARMARX_INFO << VAROUT(p.markerSize);
 
        p.visuEnabled = getProperty<bool>("visu.Enabled").getValue();
 
        {
            std::string propName = "cam.ExtraDistortionCoeffs";
            std::vector<std::string> coeffsStr;
            getProperty(coeffsStr, "cam.ExtraDistortionCoeffs");
 
            p.extraDistortionCoeffs.clear();
            for (const auto& coeffStr : coeffsStr)
            {
                try
                {
                    p.extraDistortionCoeffs.push_back(std::stof(coeffStr));
                }
                catch (const std::invalid_argument&)
                {
                    ARMARX_WARNING << "Could not parse '" << coeffStr << "' as float in property "
                                   << propName << "."
                                   << "\nIgnoring extra parameters.";
                    p.extraDistortionCoeffs.clear();
                    break;
                }
            }
        }
    }
 
    void
    ArMarkerLocalizerOpenCV::onConnectImageProcessor()
    {
        ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
 
        robot = virtualRobotReaderPlugin->get().getRobot(p.agentName);
        ARMARX_CHECK_NOT_NULL(robot) << p.agentName;
 
        referenceNode = robot->getRobotNode(p.referenceFrame);
        ARMARX_CHECK_NOT_NULL(referenceNode) << p.referenceFrame;
 
        ARMARX_VERBOSE << "Marker size: " << p.markerSize;
 
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(p.imageProviderName);
        StereoCalibrationInterfacePrx calibrationProvider =
            StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
        std::unique_ptr<CStereoCalibration> stereoCalibration(
            visionx::tools::convert(calibrationProvider->getStereoCalibration()));
        CCalibration::CCameraParameters ivtCameraParameters =
            stereoCalibration->GetLeftCalibration()->GetCameraParameters();
 
        this->cameraMatrix = visionx::makeCameraMatrix(ivtCameraParameters);
        ARMARX_VERBOSE << "Camera matrix: \n" << this->cameraMatrix;
 
        {
            // "Vector of distortion coefficients (k1,k2,p1,p2 [,k3 [,k4,k5,k6], [s1,s2,s3,s4]]) of 4, 5, 8 or 12 elements"
            // Source: https://docs.opencv.org/3.2.0/d9/d6a/group__aruco.html#ga7da45d2e8139504f3a532d884b4fb4ac
 
            std::set<int> allowedSizes = {4, 5, 8, 12};
            ARMARX_VERBOSE << "Got " << p.extraDistortionCoeffs.size()
                           << " extra distortion coefficients.";
 
            int num = int(4 + p.extraDistortionCoeffs.size());
            ARMARX_CHECK_POSITIVE(allowedSizes.count(num))
                << "Allowed sizes: "
                << simox::alg::join(
                       simox::alg::multi_to_string(allowedSizes.begin(), allowedSizes.end()), " ")
                << "\n num = " << num << " = 4 + " << p.extraDistortionCoeffs.size()
                << " = 4 + #extra coeffs";
 
            cv::Mat distortionParameters(num, 1, cv::DataType<float>::type);
 
            if (!calibrationProvider->getImagesAreUndistorted())
            {
                // k1, k2, p1, p2
                for (int i = 0; i < 4; ++i)
                {
                    distortionParameters.at<float>(i, 0) = ivtCameraParameters.distortion[i];
                }
                // extra
                for (int i = 0; size_t(i) < p.extraDistortionCoeffs.size(); ++i)
                {
                    distortionParameters.at<float>(4 + i, 0) =
                        p.extraDistortionCoeffs.at(size_t(i));
                }
            }
            else
            {
                for (int i = 0; i < distortionParameters.rows; ++i)
                {
                    distortionParameters.at<float>(i, 0) = 0;
                }
            }
 
            this->distortionCoeffs = distortionParameters;
            ARMARX_CHECK_POSITIVE(allowedSizes.count(this->distortionCoeffs.rows));
        }
        ARMARX_VERBOSE << "Distortion coefficients: \n" << this->distortionCoeffs;
 
 
        cameraImages = new CByteImage*[2];
 
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }
 
    void
    ArMarkerLocalizerOpenCV::process()
    {
        if (waitForImages(500))
        {
            armarx::MetaInfoSizeBasePtr info;
            getImages(p.imageProviderName, cameraImages, info);
            m_timestamp_last_image =
                armarx::DateTime(armarx::Duration::MicroSeconds(info->timeProvided));
 
            // getImages(cameraImages);
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error in wait for images.";
            return;
        }
 
        ArMarkerLocalizationResultList result = localizeAllMarkersInternal();
        {
            std::unique_lock lock(resultMutex);
            lastLocalizationResult = result;
            // m_timestamp_last_result = m_timestamp_last_image;
        }
 
        {
            armarx::viz::Layer layer = arviz.layer("Marker Poses");
            if (p.visuEnabled)
            {
                const Eigen::Isometry3f global_T_frame{referenceNode->getGlobalPose()};
 
                for (const auto& r : result)
                {
                    const Eigen::Isometry3f frame_T_marker{
                        armarx::FramedPosePtr::dynamicCast(r.pose)->toEigen()};
                    const Eigen::Isometry3f global_T_marker = global_T_frame * frame_T_marker;
 
                    layer.add(
                        armarx::viz::Pose(std::to_string(r.id)).pose(global_T_marker.matrix()));
                }
            }
            arviz.commit({layer});
        }
    }
 
    ArMarkerLocalizationResultList
    ArMarkerLocalizerOpenCV::localizeAllMarkersInternal()
    {
        ARMARX_TRACE;
        ARMARX_CHECK(
            virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
 
        cameraImages[0]->bytesPerPixel = 3;
        cameraImages[1]->bytesPerPixel = 3;
 
        IplImage* imageIpl = IplImageAdaptor::Adapt(cameraImages[p.cameraIndex]);
        cv::Mat imageOpenCV = cv::cvarrToMat(imageIpl);
 
        // cv::imshow("Input", imageOpenCV);
        // cv::waitKey(10);
 
        // Detect markers in image.
 
        ARMARX_DEBUG << "Calling marker detection";
 
        std::vector<int> markerIDs;
        std::vector<std::vector<cv::Point2f>> markerCorners;
        cv::aruco::detectMarkers(
            imageOpenCV, arucoDictionary, markerCorners, markerIDs, arucoParameters);
 
 
        ARMARX_CHECK_EQUAL(markerIDs.size(), markerCorners.size());
 
        ARMARX_VERBOSE << "Localized " << markerIDs.size() << " markers: "
                       << simox::alg::join(simox::alg::multi_to_string(markerIDs), " ");
 
        // We could draw an visu image:
        cv::Mat outputImage = imageOpenCV.clone();
        if (not markerIDs.empty())
        {
            cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIDs);
        }
        // cv::aruco::drawDetectedMarkers(imageOpenCV, markerCorners, markerIDs);
 
 
        // Estimate marker poses.
        ARMARX_DEBUG << "Estimating marker poses.";
 
        std::vector<cv::Vec3d> rvecs, tvecs;
        cv::aruco::estimatePoseSingleMarkers(
            markerCorners, p.markerSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
        ARMARX_CHECK_EQUAL(rvecs.size(), markerCorners.size());
        ARMARX_CHECK_EQUAL(tvecs.size(), markerCorners.size());
        ARMARX_VERBOSE << "Estimated " << rvecs.size() << " marker poses.";
 
        // Assemble results
 
        visionx::ArMarkerLocalizationResultList resultList;
 
        for (size_t i = 0; i < markerIDs.size(); ++i)
        {
            visionx::ArMarkerLocalizationResult& result = resultList.emplace_back();
 
            const cv::Vec3d& tvec = tvecs.at(i);
            Eigen::Vector3f position = Eigen::Vector3d(tvec(0), tvec(1), tvec(2)).cast<float>();
 
            cv::Mat cvOrientation;
            try
            {
                cv::Rodrigues(rvecs.at(i), cvOrientation);
            }
            catch (const std::exception& e)
            {
                ARMARX_ERROR << "Caught exception when running cv::aruco::Rodrigues(): \n"
                             << e.what();
                return {};
            }
            ARMARX_CHECK_EQUAL(cvOrientation.rows, 3);
            ARMARX_CHECK_EQUAL(cvOrientation.cols, 3);
 
            Eigen::Matrix3d orientation;
            cv::cv2eigen(cvOrientation, orientation);
 
            result.pose = new armarx::FramedPose(
                orientation.cast<float>(), position, p.referenceFrame, p.agentName);
            result.id = markerIDs.at(i);
 
            armarx::core::time::toIce(result.timestamp, m_timestamp_last_image);
 
            cv::drawFrameAxes(outputImage, cameraMatrix, distortionCoeffs, rvecs[i], tvecs[i], 0.1);
        }
 
        CByteImage* output_cimage =
            new CByteImage(outputImage.size[0], outputImage.size[1], CByteImage::ImageType::eRGB24);
        copyCvMatToIVT(outputImage, output_cimage);
 
        CByteImage* result_images[2] = {cameraImages[0], output_cimage};
        provideResultImages(result_images);
        delete output_cimage;
        return resultList;
    }
 
    visionx::ArMarkerLocalizationResultList
    visionx::ArMarkerLocalizerOpenCV::LocalizeAllMarkersNow(const Ice::Current&)
    {
        if (waitForImages(500))
        {
            getImages(cameraImages);
            return localizeAllMarkersInternal();
        }
        else
        {
            ARMARX_WARNING << "No new Images available!";
            return {};
        }
    }
 
    ArMarkerLocalizationResultList
    ArMarkerLocalizerOpenCV::GetLatestLocalizationResult(const Ice::Current&)
    {
        std::unique_lock lock(resultMutex);
        return lastLocalizationResult;
    }
 
    void
    ArMarkerLocalizerOpenCV::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(p.markerSize);
 
        tab.visuEnabled.setValue(p.visuEnabled);
 
        grid.add(Label("Marker size:"), {row, 0}).add(tab.markerSize, {row, 1});
        row++;
        grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
        row++;
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    ArMarkerLocalizerOpenCV::RemoteGui_update()
    {
        if (tab.markerSize.hasValueChanged())
        {
            p.markerSize = tab.markerSize.getValue();
        }
        if (tab.visuEnabled.hasValueChanged())
        {
            p.visuEnabled = tab.visuEnabled.getValue();
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::ArMarkerLocalizerOpenCV, ::visionx::ArMarkerLocalizerOpenCV::GetDefaultName());
 
} // namespace visionx