ArMarkerExternalCameraCalibration.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    Imagine::ArmarXObjects::ArMarkerExternalCameraCalibration
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArMarkerExternalCameraCalibration.h"
 
#include <filesystem>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr
    ArMarkerExternalCameraCalibration::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->component(localizer);
        defs->defineOptionalProperty<std::string>("cmp.PointCloudToArViz",
                                                  "PointCloudToArViz",
                                                  "Name of the PointCloudToArViz component.");
 
        defs->optional(localizerMarkerID, "loc.MarkerID", "The marker ID to use (-1 for any ID).");
 
        defs->optional(robotArMarkerFileName,
                       "robot.ArMarkerRobotFileName",
                       "File name of robot model with ArMarker marker.");
        defs->optional(robotBaseNodeName, "robot.BaseNodeName", "Name of the base robot node.");
        defs->optional(robotArMarkerMarkerNodeName,
                       "robot.ArMarkerMarkerNodeName",
                       "Name of the ArMarker marker robot node.");
 
        defs->optional(calibrationFilePath, "calibrationFilePath", "Path to the calibration file.");
 
        return defs;
    }
 
    std::string
    ArMarkerExternalCameraCalibration::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    ArMarkerExternalCameraCalibration::GetDefaultName()
    {
        return "ArMarkerExternalCameraCalibration";
    }
 
    void
    ArMarkerExternalCameraCalibration::onInitComponent()
    {
        namespace fs = std::filesystem;
 
        {
            robotArMarkerFileName = ArmarXDataPath::resolvePath(robotArMarkerFileName);
            ARMARX_INFO << "Loading robot with ArMarker marker from: " << robotArMarkerFileName;
            robotArMarker = VirtualRobot::RobotIO::loadRobot(
                robotArMarkerFileName, VirtualRobot::RobotIO::RobotDescription::eStructure);
        }
 
        {
            fs::path path = calibrationFilePath;
            fs::path parent_path = ArmarXDataPath::resolvePath(path.parent_path());
            this->calibrationFilePath = parent_path / path.filename();
            ARMARX_INFO << "Calibration file: " << calibrationFilePath;
        }
    }
 
    void
    ArMarkerExternalCameraCalibration::onConnectComponent()
    {
        getProxyFromProperty(vis.pointCloudToArViz, "cmp.PointCloudToArViz", false, "", false);
        if (vis.pointCloudToArViz)
        {
            ARMARX_INFO << "Found PointCloudToArViz.";
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
 
        {
            vis.layerCamera = arviz.layer("In Camera");
            vis.layerCamera.add(viz::Pose("Origin"));
            vis.layerCamera.add(vis.markerPoseInCamera);
 
            vis.layerRobotBase = arviz.layer("In RobotBase");
            vis.layerRobotBase.add(vis.markerPoseInRobotBase);
            vis.layerRobotBase.add(vis.markerModelInRobotBase);
            vis.layerRobotBase.add(vis.cameraPoseInRobotBase);
        }
 
 
        if (task)
        {
            task->stop();
            task->join();
            task = nullptr;
        }
        task = new SimpleRunningTask<>([this]() { this->run(); });
        task->start();
    }
 
    void
    ArMarkerExternalCameraCalibration::onDisconnectComponent()
    {
        task->stop();
        localizer = nullptr;
    }
 
    void
    ArMarkerExternalCameraCalibration::onExitComponent()
    {
    }
 
    void
    ArMarkerExternalCameraCalibration::run()
    {
        CycleUtil cycle(50);
        while (!task->isStopped())
        {
            if (!localizer)
            {
                break; // Localizer disconnected. Wait for reconnect.
            }
 
            visionx::ArMarkerLocalizationResultList localizationResults;
            try
            {
                localizationResults = localizer->GetLatestLocalizationResult();
            }
            catch (const Ice::ConnectionRefusedException&)
            {
                break; // Localizer disconnected. Wait for reconnect.
            }
 
            ARMARX_VERBOSE << "Got " << localizationResults.size() << " localization results.";
 
            std::optional<visionx::ArMarkerLocalizationResult> result;
            if (localizationResults.size() > 0)
            {
                if (localizerMarkerID < 0)
                {
                    result = localizationResults.front();
                }
                else
                {
                    for (const auto& r : localizationResults)
                    {
                        if (r.id == localizerMarkerID)
                        {
                            result = r;
                            break;
                        }
                    }
                }
            }
 
            if (result)
            {
                ARMARX_VERBOSE << "Using marker with id " << result->id;
                FramedPosePtr framedPose = FramedPosePtr::dynamicCast(result->pose);
                Eigen::Matrix4f pose = framedPose->toEigen();
 
                onLocalizationResult(pose);
            }
            else
            {
                vis.markerPoseInCamera.scale(0.5);
            }
 
            arviz.commit({vis.layerCamera, vis.layerRobotBase});
 
            cycle.waitForCycleDuration();
        }
    }
 
    void
    ArMarkerExternalCameraCalibration::onLocalizationResult(
        const Eigen::Matrix4f& markerPoseInCamera)
    {
        RobotState::synchronizeLocalClone(robotArMarker);
 
        Eigen::Matrix4f cameraPoseMarker = simox::math::inverted_pose(markerPoseInCamera);
 
        VirtualRobot::RobotNodePtr arMarkerNode =
            robotArMarker->getRobotNode(robotArMarkerMarkerNodeName);
        VirtualRobot::RobotNodePtr baseNode = robotArMarker->getRobotNode(robotBaseNodeName);
 
        FramedPose framedCameraPose(
            cameraPoseMarker, arMarkerNode->getName(), robotArMarker->getName());
        framedCameraPose.changeFrame(robotArMarker, baseNode->getName());
 
        Eigen::Matrix4f cameraPoseInRobotBase = framedCameraPose.toEigen();
        Eigen::Matrix4f markerInRobotBase = cameraPoseInRobotBase * markerPoseInCamera;
 
        vis.markerPoseInCamera.pose(markerPoseInCamera).scale(1.0);
        vis.markerPoseInRobotBase.pose(baseNode->getPoseInRootFrame() * markerInRobotBase);
        vis.cameraPoseInRobotBase.pose(baseNode->getPoseInRootFrame() * cameraPoseInRobotBase);
        {
            FramedPose framedMarkerModelPose(
                Eigen::Matrix4f::Identity(), arMarkerNode->getName(), robotArMarker->getName());
            framedMarkerModelPose.changeFrame(robotArMarker, baseNode->getName());
            vis.markerModelInRobotBase.pose(baseNode->getPoseInRootFrame() *
                                            framedMarkerModelPose.toEigen());
        }
 
        {
            std::scoped_lock lock(markerPoseMutex);
            this->markerPoseInCamera = markerPoseInCamera;
            this->cameraPoseInRobotBase = cameraPoseInRobotBase;
        }
        if (vis.pointCloudToArViz)
        {
            vis.pointCloudToArViz->setCustomTransform(
                new Pose(baseNode->getPoseInRootFrame() * cameraPoseInRobotBase));
        }
    }
 
    void
    ArMarkerExternalCameraCalibration::storeCalibration()
    {
        ExternalCameraCalibration calibration;
        {
            std::scoped_lock lock(markerPoseMutex);
            calibration.cameraPose = cameraPoseInRobotBase;
            calibration.cameraPoseFrame = robotBaseNodeName;
            calibration.timestamp = TimeUtil::GetTime();
        }
 
        const nlohmann::json j = calibration;
 
        ARMARX_IMPORTANT << "Writing calibration to \n"
                         << calibrationFilePath << "\n\n"
                         << j.dump(2);
        nlohmann::write_json(calibrationFilePath, j, 2);
    }
 
    void
    ArMarkerExternalCameraCalibration::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        tab.storeButton.setLabel("Store calibration");
 
        VBoxLayout root{tab.storeButton, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    ArMarkerExternalCameraCalibration::RemoteGui_update()
    {
        if (tab.storeButton.wasClicked())
        {
            storeCalibration();
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::ArMarkerExternalCameraCalibration, ::armarx::ArMarkerExternalCameraCalibration::GetDefaultName());
 
} // namespace armarx