DHParameterOptimizationLogger.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    RobotComponents::ArmarXObjects::DHParameterOptimizationLogger
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DHParameterOptimizationLogger.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <ArmarXCore/core/csv/CsvWriter.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include <VirtualRobot/Nodes/Sensor.h>
 
#include <Eigen/Geometry>
 
#include <iomanip>
#include <ctime>
 
using namespace armarx;
 
namespace fs = std::filesystem;
 
 
void DHParameterOptimizationLogger::logData(const Ice::Current&)
{
    std::vector<float> data;
    if (_logRepeatAccuracy)
    {
        data.push_back(-1.0f);
        data.push_back(-1.0f);
    }
    logData(data);
    if (!data.empty())
    {
        CsvWriter writer(_filePath, std::vector<std::string>(), true);
        writer.write(data);
        writer.close();
    }
 
    ARMARX_DEBUG << "Logging data finished";
}
 
void DHParameterOptimizationLogger::logDataWithRepeatAccuracy(const Ice::Current&)
{
    if (!_logRepeatAccuracy)
    {
        ARMARX_WARNING << "Loging of repeatAccuracy is not activated!";
        return;
    }
 
    std::vector<float> data;
    // placeholder for error of repeat accuracy
    data.push_back(0.0f);
    data.push_back(0.0f);
    logData(data);
    std::pair<float, float> repeatErrors;
    repeatErrors = calculateRepeatError();
    data[0] = repeatErrors.first;
    data[1] = repeatErrors.second;
 
    if (data.size() > 2)
    {
        CsvWriter writer(_filePath, std::vector<std::string>(), true);
        writer.write(data);
        writer.close();
    }
 
    ARMARX_DEBUG << "Logging data with repeat accuracy finished";
}
 
void DHParameterOptimizationLogger::startViconLogging(const Ice::Current&)
{
    _viconLoggingActive = true;
}
 
void DHParameterOptimizationLogger::stopViconLogging(const Ice::Current&)
{
    _viconLoggingActive = false;
}
 
void DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
        const std::map<std::string, std::string>& neckMarkerMapping,
        const std::map<std::string, std::string>& handMarkerMapping,
        const std::string& loggingFileName,
        bool logRepeatAccuracy,
        const Ice::Current&)
{
    ARMARX_CHECK_EXPRESSION(!neckMarkerMapping.empty());
    ARMARX_CHECK_EXPRESSION(!handMarkerMapping.empty());
    ARMARX_CHECK_EXPRESSION(!kinematicChainName.empty());
 
    _neckMarkerMapping = neckMarkerMapping;
    _handMarkerMapping = handMarkerMapping;
 
    _neckMarker.clear();
    _handMarker.clear();
 
    for (auto& pair : neckMarkerMapping)
    {
        _neckMarker.push_back(pair.first);
    }
    for (auto& pair : handMarkerMapping)
    {
        _handMarker.push_back(pair.first);
    }
    for (auto& s : _neckMarker)
    {
        if (std::find(_handMarker.begin(), _handMarker.end(), s) != _handMarker.end())
        {
            ARMARX_ERROR << "HandMarker and NeckMarker can not contain the same marker!! Abort.";
            return;
        }
    }
    std::vector<VirtualRobot::SensorPtr> sensors = localRobot->getSensors();
    for (auto& pair : neckMarkerMapping)
    {
        auto& i = pair.second;
        ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
        {
            return s->getName() == i;
        }) != sensors.end()) <<
                             "Robot does not have a sensor with name '" + i + "'";
    }
    for (auto& pair : handMarkerMapping)
    {
        auto& i = pair.second;
        ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
        {
            return s->getName() == i;
        }) != sensors.end()) <<
                             "Robot does not have a sensor with name '" + i + "'";
    }
 
    _kcName = kinematicChainName;
    ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(_kcName)) << "Robot " + localRobot->getName() + " does not have a kinematic chain with name " + _kcName;
    _kc = localRobot->getRobotNodeSet(_kcName);
    _jointNames = _kc->getNodeNames();
    _logRepeatAccuracy = logRepeatAccuracy;
 
    setupFile(loggingFileName);
    setupHeader();
    ARMARX_INFO << VAROUT(_header);
    CsvWriter writer(_filePath, _header, true);
    writer.close();
 
    _initialized = true;
}
 
void DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap, const Ice::Current&)
{
    // Format of received data: ObjectName:MarkerName x y z
 
    StringVector3fMap copyMap;
    for (auto& it : markerPosMap)
    {
        std::string key = it.first;
        int i = key.find(':') + 1;
        copyMap[key.erase(0, i)] = it.second;
        Vector3f v = it.second;
        //        ARMARX_INFO << it.first << " " << v.e0 << " " << v.e1 << " " << v.e2;
        debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_x", new Variant(v.e0));
        debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_y", new Variant(v.e1));
        debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_z", new Variant(v.e2));
    }
 
    if (_viconLoggingActive)
    {
        std::unique_lock lock(_bufferMutex);
        _viconMarkerBuffer = copyMap;
        _timeStampLastViconDataUpdate = TimeUtil::GetTime();
        _viconBufferUpdated = true;
 
    }
}
 
void DHParameterOptimizationLogger::reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos, const Ice::Current&)
{
    // Not needed
}
 
void DHParameterOptimizationLogger::setupFile(const std::string& fileName)
{
    std::string localFileName = fileName;
    using namespace std::filesystem;
 
    auto timeToString = []()
    {
        std::time_t now = std::time(nullptr);
        std::tm* nowTm = std::localtime(&now);
 
        std::stringstream ss;
        ss << std::put_time(nowTm, "%Y%m%d_%H%M%S");
        return ss.str();
    };
 
    // Setup logging file
    if (ends_with(localFileName, ".csv"))
    {
        localFileName = localFileName.substr(0, localFileName.size() - 4);
    }
    localFileName += "_";
    localFileName += timeToString();
    localFileName += ".csv";
 
    std::string pathOrig = getProperty<std::string>("LoggingFilePath").getValue();
    if (!pathOrig.empty() && pathOrig[0] == '~')
    {
        ARMARX_CHECK_EXPRESSION(pathOrig.size() == 1 || pathOrig[1] == '/') << "LoggingFilePath isviconValues not valid: '~' has to be followed by ’/’";
        char const* home = std::getenv("HOME");
        ARMARX_CHECK_EXPRESSION(home) << "HOME environment variable is not set!";
        pathOrig.replace(0, 1, home);
    }
    fs::path filePath(pathOrig);
    if (!fs::exists(filePath))
    {
        fs::create_directory(filePath);
    }
    filePath /= path(localFileName);
    _filePath = filePath.string();
}
 
void DHParameterOptimizationLogger::onInitComponent()
{
    usingTopic(getProperty<std::string>("ViconDataTopicName").getValue());
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
    usingProxy(getProperty<std::string>("RobotUnitObserverName").getValue());
 
    _initialized = false;
 
    int size = getProperty<int>("FilterWindowSize").getValue();
    ARMARX_CHECK_EXPRESSION(size > 0);
    _filterSize = size;
 
    _viconObjectName_neck = getProperty<std::string>("Neck_ViconObjectName").getValue();
    _viconObjectName_hand = getProperty<std::string>("Hand_ViconObjectName").getValue();
}
 
 
void DHParameterOptimizationLogger::onConnectComponent()
{
    getProxy(robotStateComponent, getProperty<std::string>("RobotStateComponentName").getValue());
    getProxy(robotUnitObserver, getProperty<std::string>("RobotUnitObserverName").getValue());
    _viconLoggingActive = true;
 
    debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
 
    localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
 
    std::vector<VirtualRobot::SensorPtr> s = localRobot->getSensors();
    for (auto& sp : s)
    {
        ARMARX_INFO << sp->getName();
    }
}
 
 
void DHParameterOptimizationLogger::onDisconnectComponent()
{
    _viconLoggingActive = false;
}
 
 
void DHParameterOptimizationLogger::onExitComponent()
{
 
}
 
armarx::PropertyDefinitionsPtr DHParameterOptimizationLogger::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new DHParameterOptimizationLoggerPropertyDefinitions(
            getConfigIdentifier()));
}
 
void DHParameterOptimizationLogger::setupHeader()
{
    std::vector<std::string> header;
 
    // order is important!! do not change without adapting the order of logging calls in logData(std::vector<std::string> &data) !!
 
    // Accuracy columns
    if (_logRepeatAccuracy)
    {
        header.push_back("Error_Translation");
        header.push_back("Error_Orientation");
    }
 
    // ViconMarker columns neck --> 12
    for (auto& m : _neckMarker)
    {
        header.push_back(m + "_x");
        header.push_back(m + "_y");
        header.push_back(m + "_z");
    }
 
    // ViconMarker columns hand --> 12
    for (auto& m : _handMarker)
    {
        header.push_back(m + "_x");
        header.push_back(m + "_y");
        header.push_back(m + "_z");
    }
 
    // JointValues --> ~8
    for (auto& node : _jointNames)
    {
        header.push_back("JointValue_" + node);
    }
    header.push_back("JointValue_TorsoJoint");
 
    // ForwardKinematic to vicon marker --> 12
    for (auto& m : _handMarker)
    {
        header.push_back("FK_" + m + "_x");
        header.push_back("FK_" + m + "_y");
        header.push_back("FK_" + m + "_z");
    }
 
    // Error between model and vicon data --> 2
    header.push_back("Error_translation");
    header.push_back("Error_orientation");
 
    // Raw torque ticks --> ~8
    for (auto& node : _jointNames)
    {
        header.push_back(node + "_torqueTicks");
    }
 
    // TorqueValues --> ~8
    for (auto& node : _jointNames)
    {
        header.push_back(node + "_torque");
    }
 
    // FT Sensor torque --> 3
    if (_kc->getTCP()->getName() == "Hand R TCP")
    {
        header.push_back("FT R_FT_torque_x");
        header.push_back("FT R_FT_torque_y");
        header.push_back("FT R_FT_torque_z");
    }
    else if (_kc->getTCP()->getName() == "Hand L TCP")
    {
        header.push_back("FT L_FT_torque_x");
        header.push_back("FT L_FT_torque_y");
        header.push_back("FT L_FT_torque_z");
    }
 
    _header = header;
}
 
void DHParameterOptimizationLogger::logData(std::vector<float>& data)
{
    // order is important!! do not change without adapting the order of setting up the header in setupHeader() !!
    logViconMarkerPositions(data);
    logJointValues(data);
    logForwardKinematicToViconMarker(data);
    logErrorBetweenModelAndVicon(data);
    logRawTorqueTicks(data);
    logTorqueValues(data);
    logFTSensorValues(data);
}
 
void DHParameterOptimizationLogger::logViconMarkerPositions(std::vector<float>& data)
{
    ARMARX_DEBUG << "Logging viconMarkerPositions";
 
    if (!_viconLoggingActive)
    {
        ARMARX_DEBUG << deactivateSpam(1) << "ViconLogging not active";
        size_t sum = 3 * (_neckMarker.size() + _handMarker.size());
        for (size_t i = 0; i < sum; i++)
        {
            data.push_back(0.0f);
        }
    }
    else
    {
        // blocking wait until all marker are visible --> no marker has 0 values, and enough data frames for filtering arrived
        StringVector3fMap values = waitBlockingForAllMarkersFiltered();
        if (values.empty())
        {
            size_t sum = 3 * (_neckMarker.size() + _handMarker.size());
            for (size_t i = 0; i < sum; i++)
            {
                data.push_back(0.0f);
            }
        }
        else
        {
            values = transformViconMarkerPositionsIntoRobotRootFrame(values);
            for (auto& m : _neckMarker)
            {
                data.push_back(values.at(m).e0);
                data.push_back(values.at(m).e1);
                data.push_back(values.at(m).e2);
            }
            for (auto& m : _handMarker)
            {
                data.push_back(values.at(m).e0);
                data.push_back(values.at(m).e1);
                data.push_back(values.at(m).e2);
            }
 
            _filteredViconMarkerPositions_robotRootFrame = values;
            _filteredViconMarkerPositionsUpdated = true;
        }
    }
}
 
void DHParameterOptimizationLogger::logJointValues(std::vector<float>& data) const
{
    ARMARX_DEBUG << "Logging jointValues";
 
    RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
    for (auto& node : _kc->getAllRobotNodes())
    {
        data.push_back(node->getJointValue());
    }
    data.push_back(localRobot->getRobotNode("TorsoJoint")->getJointValue());
}
 
void DHParameterOptimizationLogger::logForwardKinematicToViconMarker(std::vector<float>& data) const
{
    ARMARX_DEBUG << "Logging forwardKinematicToViconMarker";
 
    RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
    VirtualRobot::RobotNodePtr hand = getHandMarkerRootNode();
 
    for (VirtualRobot::SensorPtr const& sensor : hand->getSensors())
    {
        Eigen::Matrix4f m = sensor->getTransformationFrom(localRobot->getRootNode());
        data.push_back(m(0, 3)); // x
        data.push_back(m(1, 3)); // y
        data.push_back(m(2, 3)); // z
    }
}
 
void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<float>& data)
{
    ARMARX_DEBUG << "Logging errorBetweenModelAndVicon";
 
    if (!_viconLoggingActive || !_filteredViconMarkerPositionsUpdated)
    {
        data.push_back(0.0f);
        data.push_back(0.0f);
    }
    else
    {
        RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
        VirtualRobot::RobotNodePtr neck = getNeckMarkerRootNode();
        VirtualRobot::RobotNodePtr hand = getHandMarkerRootNode();
        VirtualRobot::RobotNodePtr root = localRobot->getRootNode();
 
        _lastObservedHandRootPose = _newObservedHandRootPose;
 
        //        Eigen::Matrix4f modelRelativePose = hand->getTransformationFrom(neck);
 
        // Calculation of neck pose from vicon data
        Eigen::MatrixXf observedNeckPositions_robotRootFrame(3, _neckMarker.size());
        Eigen::MatrixXf modelNeckPositions_neckFrame(3, _neckMarker.size());
 
 
 
 
 
        size_t i = 0;
        for (auto& it : _neckMarker)
        {
            observedNeckPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
            observedNeckPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
            observedNeckPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
 
            modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
            modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
            modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
 
            // visu
            Eigen::Vector3f position = (root->getGlobalPose() * observedNeckPositions_robotRootFrame).block<3, 1>(0, i);
            debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 1, 1}, 7);
 
            i++;
        }
 
 
 
        // Calculation of hand pose from vicon data
        Eigen::MatrixXf observedHandPositions_robotRootFrame(3, _handMarker.size());
        Eigen::MatrixXf modelHandPositions_handFrame(3, _handMarker.size());
 
 
        i = 0;
        for (auto& it : _handMarker)
        {
            observedHandPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
            observedHandPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
            observedHandPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
 
            modelHandPositions_handFrame(0, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(0, 3);
            modelHandPositions_handFrame(1, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(1, 3);
            modelHandPositions_handFrame(2, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(2, 3);
 
            // visu
            Eigen::Vector3f position = (root->getGlobalPose() * observedHandPositions_robotRootFrame).block<3, 1>(0, i);
            debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 0, 1}, 7);
 
 
            i++;
        }
 
 
        ARMARX_DEBUG << VAROUT(modelHandPositions_handFrame) << VAROUT(observedHandPositions_robotRootFrame);
        Eigen::Matrix4f observedHandRootPose_robotRootFrame = registration3d(modelHandPositions_handFrame.transpose(), observedHandPositions_robotRootFrame.transpose());
 
        _newObservedHandRootPose = observedHandRootPose_robotRootFrame;
        _observedHandPosesUpdated = true;
 
        debugDrawerPrx->setPoseVisu("MarkerVisu", "observedTCP", new Pose(root->getGlobalPose() * observedHandRootPose_robotRootFrame));
        debugDrawerPrx->setPoseVisu("MarkerVisu", "kinematicTCP", new Pose(hand->getGlobalPose()));
 
        //        Eigen::Matrix4f viconRelativePose = observedNeckRootPose.inverse() * observedHandRootPose;
        float errorPos = (hand->getPoseInRootFrame().block<3, 1>(0, 3) - observedHandRootPose_robotRootFrame.block<3, 1>(0, 3)).norm();
        // Calculate error
        //        Eigen::Vector3f errorV = viconRelativePose.block<3, 1>(0, 3) - modelRelativePose.block<3, 1>(0, 3);
        //        float errorPos = errorV.norm();
 
        Eigen::Matrix3f oriDir = hand->getPoseInRootFrame().block<3, 3>(0, 0) * observedHandRootPose_robotRootFrame.block<3, 3>(0, 0).inverse();
        Eigen::AngleAxisf aa(oriDir);
        float errorOri = aa.angle();
 
        ARMARX_INFO << "Error (Hand) in rootFrame between model and vicon ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
 
        data.push_back(errorPos);
        data.push_back(errorOri);
 
        _filteredViconMarkerPositionsUpdated = false;
    }
}
 
void DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data) const
{
    ARMARX_DEBUG << "Logging rawTorqueTicks";
 
    if (robotUnitObserver->existsChannel("SensorDevices"))
    {
        for (auto& node : _jointNames)
        {
            if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks"))
            {
                data.push_back((float) robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks")->getInt());
            }
            else
            {
                data.push_back(0.0f);
            }
        }
    }
    else
    {
        for (size_t i = 0; i < _jointNames.size(); i++)
        {
            data.push_back(0.0f);
        }
    }
}
 
void DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) const
{
    ARMARX_DEBUG << "Logging torqueValues";
 
    if (robotUnitObserver->existsChannel("SensorDevices"))
    {
        for (auto& node : _jointNames)
        {
            if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torque"))
            {
                data.push_back(robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torque")->getFloat());
            }
            else
            {
                data.push_back(0.0f);
            }
        }
    }
    else
    {
        for (size_t i = 0; i < _jointNames.size(); i++)
        {
            data.push_back(0.0f);
        }
    }
}
 
void DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data) const
{
    ARMARX_DEBUG << "Logging FTSensorValues";
 
    std::string datafieldName ;
    if (_kc->getTCP()->getName() == "Hand R TCP")
    {
        datafieldName = "FT R_FTSensorDataValue_torque";
    }
    else if (_kc->getTCP()->getName() == "Hand L TCP")
    {
        datafieldName = "FT L_FTSensorDataValue_torque";
    }
 
    if (robotUnitObserver->existsChannel("SensorDevices"))
    {
        if (robotUnitObserver->existsDataField("SensorDevices", datafieldName))
        {
            TimedVariantPtr tv = TimedVariantPtr::dynamicCast(robotUnitObserver->getDatafieldByName("SensorDevices", datafieldName));
            ARMARX_CHECK_EXPRESSION(tv->getType() == VariantType::Vector3);
            Vector3Ptr v = tv->get<Vector3>();
 
            data.push_back(v->x);
            data.push_back(v->y);
            data.push_back(v->z);
        }
        else
        {
            data.push_back(0.0f);
            data.push_back(0.0f);
            data.push_back(0.0f);
        }
    }
    else
    {
        data.push_back(0.0f);
        data.push_back(0.0f);
        data.push_back(0.0f);
    }
}
 
StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFiltered()
{
    TIMING_START(waitBlockingForAllMarkersFiltered);
    std::map<std::string, armarx::rtfilters::AverageFilter> viconDataFilters;
 
    // Setup filter
    viconDataFilters.clear();
    for (auto& s : _neckMarker)
    {
        viconDataFilters.insert(std::make_pair(s + "_x", rtfilters::AverageFilter(_filterSize)));
        viconDataFilters.insert(std::make_pair(s + "_y", rtfilters::AverageFilter(_filterSize)));
        viconDataFilters.insert(std::make_pair(s + "_z", rtfilters::AverageFilter(_filterSize)));
    }
    for (auto& s : _handMarker)
    {
        viconDataFilters.insert(std::make_pair(s + "_x", rtfilters::AverageFilter(_filterSize)));
        viconDataFilters.insert(std::make_pair(s + "_y", rtfilters::AverageFilter(_filterSize)));
        viconDataFilters.insert(std::make_pair(s + "_z", rtfilters::AverageFilter(_filterSize)));
    }
 
    size_t i = 0; // Counter to make sure the whole filter is filled
    StringVector3fMap viconValues;
    bool viconValuesSet = false;
    while (_viconLoggingActive && !viconValuesSet)
    {
        // Waiting for new vicon data
        if (!_viconBufferUpdated)
        {
            TimeUtil::USleep(10);
        }
        else
        {
            _viconBufferUpdated = false;
            // copying viconData
            {
                std::unique_lock lock(_bufferMutex);
                viconValues = _viconMarkerBuffer;
            }
            // Check if all data is valid, if not, continue waiting for next data frame
            for (const std::string& s : _neckMarker)
            {
                ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0) << armarx::getMapKeys(viconValues) << "\n" << s;
                Vector3f v = viconValues.at(s);
                if (v.e0 == 0.0f && v.e1 == 0.0f && v.e2 == 0.0f)
                {
                    goto invalidData; // rare case where goto is actually useful
                }
            }
            for (const std::string& s : _handMarker)
            {
                ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0);
                Vector3f v = viconValues.at(s);
                if (v.e0 == 0.0f && v.e1 == 0.0f && v.e2 == 0.0f)
                {
                    goto invalidData; // rare case where goto is actually useful
                }
            }
 
            // All data is valid, we can push it to the filters
            for (const std::string& s : _neckMarker)
            {
                Vector3f v = viconValues.at(s);
                viconDataFilters.at(s + "_x").update(_timeStampLastViconDataUpdate, v.e0);
                viconDataFilters.at(s + "_y").update(_timeStampLastViconDataUpdate, v.e1);
                viconDataFilters.at(s + "_z").update(_timeStampLastViconDataUpdate, v.e2);
            }
            for (const std::string& s : _handMarker)
            {
                Vector3f v = viconValues.at(s);
                viconDataFilters.at(s + "_x").update(_timeStampLastViconDataUpdate, v.e0);
                viconDataFilters.at(s + "_y").update(_timeStampLastViconDataUpdate, v.e1);
                viconDataFilters.at(s + "_z").update(_timeStampLastViconDataUpdate, v.e2);
            }
            if (i >= _filterSize)
            {
                viconValues.clear();
                for (const std::string& s : _neckMarker)
                {
                    float x = viconDataFilters.at(s + "_x").calculate();
                    float y = viconDataFilters.at(s + "_y").calculate();
                    float z = viconDataFilters.at(s + "_z").calculate();
                    Vector3f v;
                    v.e0 = x;
                    v.e1 = y;
                    v.e2 = z;
                    viconValues.insert(std::make_pair(s, v));
 
                }
                for (const std::string& s : _handMarker)
                {
                    float x = viconDataFilters.at(s + "_x").calculate();
                    float y = viconDataFilters.at(s + "_y").calculate();
                    float z = viconDataFilters.at(s + "_z").calculate();
                    Vector3f v;
                    v.e0 = x;
                    v.e1 = y;
                    v.e2 = z;
                    viconValues.insert(std::make_pair(s, v));
                }
                viconValuesSet = true;
            }
            else
            {
                i++;
            }
        }
invalidData:; // rare case where goto is actually useful
    }
    TIMING_END(waitBlockingForAllMarkersFiltered);
    viconDataFilters.clear();
    return viconValues;
}
 
VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getNeckMarkerRootNode() const
{
    ARMARX_CHECK_EXPRESSION(_kc);
    VirtualRobot::RobotNodePtr node;
    if (_kc->getTCP()->getName() == "Hand R TCP")
    {
        node = localRobot->getRobotNode("DHParameterOptimization_NeckRight");
    }
    else if (_kc->getTCP()->getName() == "Hand L TCP")
    {
        node = localRobot->getRobotNode("DHParameterOptimization_NeckLeft");
    }
    else
    {
        ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
    }
    return node;
}
 
VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getHandMarkerRootNode() const
{
    ARMARX_CHECK_EXPRESSION(_kc);
    VirtualRobot::RobotNodePtr node;
    if (_kc->getTCP()->getName() == "Hand R TCP")
    {
        node = localRobot->getRobotNode("DHParameterOptimization_HandRight");
    }
    else if (_kc->getTCP()->getName() == "Hand L TCP")
    {
        node = localRobot->getRobotNode("DHParameterOptimization_HandLeft");
    }
    else
    {
        ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
    }
    return node;
}
 
std::pair<float, float> DHParameterOptimizationLogger::calculateRepeatError()
{
    if (!_observedHandPosesUpdated)
    {
        ARMARX_WARNING << "Pose of HandViconMarker have not been updated. No repeat accuracy calculated. (writes -1.0 in log)";
        return std::make_pair(-1.0f, -1.0f);
    }
    _observedHandPosesUpdated = false;
 
    Eigen::Matrix4f oldPose = _lastObservedHandRootPose;
    Eigen::Matrix4f newPose = _newObservedHandRootPose;
 
    // Calculate error
    Eigen::Vector3f errorV = oldPose.block<3, 1>(0, 3) - newPose.block<3, 1>(0, 3);
    float errorPos = errorV.norm();
 
    Eigen::Matrix3f oriDir = oldPose.block<3, 3>(0, 0) * newPose.block<3, 3>(0, 0).inverse();
    Eigen::AngleAxisf aa(oriDir);
    float errorOri = aa.angle();
 
    ARMARX_INFO << "Repeat Accuracy: Error between old pose and new pose ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
 
    return std::make_pair(errorPos, errorOri);
}
 
 
Eigen::Matrix4f DHParameterOptimizationLogger::registration3d(const Eigen::MatrixXf& A, const Eigen::MatrixXf& B) const
{
 
    // std::cout << "A:\n" << A<< std::endl;
    // std::cout << "B:\n" << B<< std::endl;
    assert(A.rows() == B.rows()); // Same number of points
    assert(A.cols() == B.cols()); // Same dimension
    //    assert(A.cols()==3);
    assert(A.cols() == 3 || A.cols() == 4); // Homogeneous coordinates?
    assert(A.rows() >= 3);
 
 
    Eigen::Vector3f centroid_A = A.block(0, 0, A.rows(), 3).colwise().mean();
    Eigen::Vector3f centroid_B = B.block(0, 0, A.rows(), 3).colwise().mean();
    //    std::cout << "cA & cB: \n" << centroid_A << std::endl << std::endl << centroid_B << std::endl;
    Eigen::MatrixXf AA = A.block(0, 0, A.rows(), 3).rowwise() - centroid_A.transpose();
    Eigen::MatrixXf BB = (B.block(0, 0, A.rows(), 3).rowwise() - centroid_B.transpose());
    //    std::cout << VAROUT(AA) << VAROUT(BB);
    Eigen::MatrixXf H = (AA).transpose() * BB;
    //    std::cout << "H: " << H<< std::endl;
 
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3f U = svd.matrixU();
    // std::cout << "U: " << U<< std::endl;
    Eigen::Matrix3f V = svd.matrixV();
    // std::cout << "V: " << V<< std::endl;
    Eigen::Matrix3f R = V * U.transpose();
    // std::cout << "R: " << R<< std::endl;
 
    if (R.determinant() < 0)
    {
        // Reflection detected
        //        abort();
        //        ARMARX_INFO << "Reflection detected";
        V.col(2) *= -1;
        R = V * U.transpose();
        //        R.col(2) *= -1;
    }
 
    Eigen::Vector3f t = -R * centroid_A + centroid_B;
    // std::cout << "t: " << t << std::endl;
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
    T.block(0, 0, 3, 3) = R;
    T.block(0, 3, 3, 1) = t;
 
    return T;
}
 
StringVector3fMap DHParameterOptimizationLogger::transformViconMarkerPositionsIntoRobotRootFrame(const StringVector3fMap& values) const
{
    RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
    VirtualRobot::RobotNodePtr neck = getNeckMarkerRootNode();
 
    Eigen::MatrixXf observedNeckPositions_viconRootFrame(3, _neckMarker.size());
    Eigen::MatrixXf modelNeckPositions_neckFrame(3, _neckMarker.size());
 
    size_t i = 0;
    for (auto& it : _neckMarker)
    {
        observedNeckPositions_viconRootFrame(0, i) = values.at(it).e0;
        observedNeckPositions_viconRootFrame(1, i) = values.at(it).e1;
        observedNeckPositions_viconRootFrame(2, i) = values.at(it).e2;
        modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
        modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
        modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
 
        i++;
    }
    Eigen::Matrix4f observedNeckRootPose_viconRootFrame = registration3d(modelNeckPositions_neckFrame.transpose(), observedNeckPositions_viconRootFrame.transpose());
    localRobot->setGlobalPoseForRobotNode(neck, observedNeckRootPose_viconRootFrame);
    Eigen::Matrix4f realRobotToVicon = localRobot->getGlobalPose().inverse();
 
 
    //    Eigen::Matrix4f realGlobalRootPose = localRobot->getGlobalPose();
    //        ARMARX_DEBUG << VAROUT(observedNeckRootPose_robotRootFrame) << VAROUT(modelNeckPositions_neckFrame) << VAROUT(observedNeckPositions_robotRootFrame);
    //        Eigen::Matrix4f realGlobalRootPose = localRobot->getGlobalPose();
    //        localRobot->setGlobalPoseForRobotNode(neck, observedNeckRootPose_robotRootFrame);
 
    //    debugDrawerPrx->setRobotVisu("markerVisu", "robot", robotStateComponent->getRobotFilename(), "armar6_rt", FullModel);
    //    debugDrawerPrx->updateRobotPose("markerVisu", "robot", new Pose(localRobot->getGlobalPose()));
    //    debugDrawerPrx->updateRobotConfig("markerVisu", "robot", localRobot->getConfig()->getRobotNodeJointValueMap());
 
    //        Eigen::Matrix4f realRobotToVicon = realGlobalRootPose.inverse() * localRobot->getGlobalPose();
    //        ARMARX_INFO << VAROUT(realRobotToVicon) << VAROUT(realGlobalRootPose) << VAROUT(localRobot->getGlobalPose()) << VAROUT(neck->getGlobalPose()) << VAROUT(root->getGlobalPose());
 
 
 
    StringVector3fMap transformedValues;
    for (auto& it : values)
    {
        std::string markerName = it.first;
        armarx::Vector3f pos_viconRootFrame = it.second;
 
        Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
        mat(0, 3) = pos_viconRootFrame.e0;
        mat(1, 3) = pos_viconRootFrame.e1;
        mat(2, 3) = pos_viconRootFrame.e2;
 
        mat = realRobotToVicon * mat;
 
        armarx::Vector3f r;
        r.e0 = mat(0, 3);
        r.e1 = mat(1, 3);
        r.e2 = mat(2, 3);
        transformedValues[markerName] = r;
    }
 
    return transformedValues;
}