LaserScannerSelfLocalisation.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::LaserScannerSelfLocalisation
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LaserScannerSelfLocalisation.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
// These object factories are required, otherwise a runtime error will occur (static global object registration...)
#include <cfloat>
#include <chrono>
#include <fstream>
#include <iomanip>
 
#include <Eigen/Geometry>
 
#include <IceUtil/UUID.h>
 
#include <SimoxUtility/json/json.hpp>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/Gaussian.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
 
using namespace armarx;
 
using Line = Eigen::ParametrizedLine<float, 2>;
 
namespace
{
    bool
    anyIsNaN(Eigen::Vector2f const& v)
    {
        return std::isnan(v.x()) || std::isnan(v.y());
    }
 
    bool
    anyIsNaN(Eigen::Vector3f const& v)
    {
        return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z());
    }
 
    std::string
    readWholeFile(std::string const& filename)
    {
        std::ifstream t(filename.c_str());
        std::string str;
 
        t.seekg(0, std::ios::end);
        str.reserve(t.tellg());
        t.seekg(0, std::ios::beg);
 
        str.assign(std::istreambuf_iterator<char>(t), std::istreambuf_iterator<char>());
        return str;
    }
 
    Eigen::Vector2f
    parsePosition(Json::Value const& pos)
    {
        Eigen::Vector2f result = Eigen::Vector2f::Zero();
        if (pos.isArray() && pos.size() == 2)
        {
            Json::Value const& x = pos[0];
            Json::Value const& y = pos[1];
            if (x.isNumeric() && y.isNumeric())
            {
                result[0] = (float)x.asDouble();
                result[1] = (float)y.asDouble();
            }
            else
            {
                throw std::runtime_error("Map format error: Unexpected types in 2D vector");
            }
        }
        else
        {
            throw std::runtime_error(
                "Map format error: Unexpected number of elements in 2D vector");
        }
        return result;
    }
 
    std::vector<LineSegment2Df>
    loadMapFromFile(std::string const& filename)
    {
        std::string fullFilename;
        if (!ArmarXDataPath::getAbsolutePath(filename, fullFilename))
        {
            throw std::runtime_error("Could not find map file: " + filename);
        }
 
        std::string fileContent = readWholeFile(fullFilename);
        JSONObject o;
        o.fromString(fileContent);
        Json::Value const& j = o.getJsonValue();
        if (!j.isObject())
        {
            throw std::runtime_error("Map format error: expected a JSON object at root level");
        }
 
        float lengthUnit = 1.0f; // Default: mm
        Json::Value const& unitValue = j["LengthUnit"];
        if (unitValue.isString())
        {
            std::string unit = unitValue.asString();
            if (unit == "m")
            {
                lengthUnit = 1000.0f;
            }
            else if (unit == "mm")
            {
                lengthUnit = 1.0f;
            }
            else
            {
                throw std::runtime_error("Unknown length unit: " + unit);
            }
        }
 
        Json::Value const& map = j["Map"];
        if (!map.isArray())
        {
            throw std::runtime_error("Map format error: Expected an array at property 'Map'");
        }
 
        std::vector<LineSegment2Df> result;
 
        for (Json::ArrayIndex i = 0; i < map.size(); ++i)
        {
            Json::Value const& p = map[i];
            Json::Value const& position = p["Position"];
            Eigen::Vector2f pos = parsePosition(position);
            Json::Value const& strip = p["RelativeLineStrip"];
            if (!strip.isArray())
            {
                throw std::runtime_error(
                    "Map format error: Expected an error at property 'RelativeLineStrip'");
            }
 
            Eigen::Vector2f startPos = pos;
            for (Json::ArrayIndex j = 0; j < strip.size(); ++j)
            {
                Eigen::Vector2f nextPos = startPos + parsePosition(strip[j]);
 
                result.push_back(LineSegment2Df{lengthUnit * startPos, lengthUnit * nextPos});
                startPos = nextPos;
            }
        }
 
        return result;
    }
 
    float
    lineSegmentToPointDistanceSquared(LineSegment2Df const& segment, Eigen::Vector2f point)
    {
        // Return minimum distance between line segment and a point
        Eigen::Vector2f dir = segment.end - segment.start;
        float l2 = dir.squaredNorm();
        if (l2 <= 0.0)
        {
            return (point - segment.start).squaredNorm();
        }
 
        // Consider the line extending the segment, parameterized as v + t (w - v).
        // We find projection of point p onto the line.
        // It falls where t = [(p-v) . (w-v)] / |w-v|^2
        // We clamp t from [0,1] to handle points outside the segment vw.
        float t = std::max(0.0f, std::min(1.0f, (point - segment.start).dot(dir) / l2));
        Eigen::Vector2f projection = segment.start + t * dir; // Projection falls on the segment
        return (point - projection).squaredNorm();
    }
 
    float
    distance(Line const& line, Eigen::Vector2f point)
    {
        Eigen::Vector2f normalizedDir = line.direction();
        Eigen::Vector2f diff = point - line.origin();
        float signedDistance = normalizedDir.x() * diff.y() - normalizedDir.y() * diff.x();
        return std::fabs(signedDistance);
    }
 
    Line
    leastSquareLine(Eigen::Vector2f* begin,
                    Eigen::Vector2f* end,
                    float* maxError = nullptr,
                    float* averageError = nullptr,
                    float* as = nullptr,
                    float* bs = nullptr,
                    float* det = nullptr)
    {
        float sumx = 0.0f;
        float sumy = 0.0f;
        float sumxx = 0.0f;
        float sumyy = 0.0f;
        float sumxy = 0.0f;
        for (Eigen::Vector2f* point = begin; point != end; ++point)
        {
            float x = point->x();
            float y = point->y();
            sumx += x;
            sumy += y;
            sumxx += x * x;
            sumyy += y * y;
            sumxy += x * y;
        }
 
        float numx = (float)(end - begin);
        float calc_det = (numx * sumxx) - (sumx * sumx);
        float calc_dety = (numx * sumyy) - (sumy * sumy);
 
        float calc_as = (sumxx * sumy) - (sumx * sumxy);
        float calc_bs = (numx * sumxy) - (sumx * sumy);
 
        Line zeroLine;
        zeroLine.origin() = Eigen::Vector2f::Zero();
        zeroLine.direction() = Eigen::Vector2f::UnitX();
        if (std::abs(calc_as) < 0.001f && std::abs(calc_bs) < 0.001f)
        {
            ARMARX_VERBOSE << "Vector is close to zero " << VAROUT(calc_as) << " "
                           << VAROUT(calc_bs);
            return zeroLine;
        }
        if (numx == 0.0f)
        {
            ARMARX_VERBOSE << "numx is zero " << VAROUT(begin) << " " << VAROUT(end);
            return zeroLine;
        }
 
 
        Line line;
        if (calc_det > calc_dety)
        {
            line.direction() = Eigen::Vector2f(calc_det, calc_bs).normalized();
        }
        else
        {
            line.direction() = Eigen::Vector2f(calc_bs, calc_dety).normalized();
        }
 
        line.origin() = Eigen::Vector2f(sumx / numx, sumy / numx);
 
        // the error is calculated only if demanded
        if (numx > 0 && maxError)
        {
            float this_average_error = 0.0f;
            float this_max_error = 0.0f;
            for (Eigen::Vector2f* point = begin; point != end; ++point)
            {
                float point_error = distance(line, *point);
                this_average_error += point_error;
                this_max_error = std::max(point_error, this_max_error);
            }
            this_average_error = this_average_error / numx;
 
            *maxError = this_max_error;
 
            if (averageError)
            {
                *averageError = this_average_error;
            }
        }
 
        if (as)
        {
            *as = calc_as;
        }
        if (bs)
        {
            *bs = calc_bs;
        }
        if (det)
        {
            *det = calc_det;
        }
 
        return line;
    }
 
    LineSegment2Df
    leastSquareLineSegmentInOrder(Eigen::Vector2f* begin,
                                  Eigen::Vector2f* end,
                                  float* maxError,
                                  float* averageError)
    {
        LineSegment2Df segment;
        if ((end - begin) < 2)
        {
            segment.start = Eigen::Vector2f::Zero();
            segment.end = Eigen::Vector2f(5000.0f, 5000.0f);
            if (maxError)
            {
                *maxError = FLT_MAX;
            }
            if (averageError)
            {
                *averageError = FLT_MAX;
            }
            return segment;
        }
 
        Line line = leastSquareLine(begin, end, maxError, averageError, nullptr, nullptr, nullptr);
        if (anyIsNaN(line.direction()))
        {
            segment.start = Eigen::Vector2f::Zero();
            segment.end = Eigen::Vector2f(5000.0f, 5000.0f);
            if (maxError)
            {
                *maxError = FLT_MAX;
            }
            if (averageError)
            {
                *averageError = FLT_MAX;
            }
            return segment;
        }
        segment.start = line.projection(*begin);
        segment.end = line.projection(*(end - 1));
 
        if (anyIsNaN(segment.start))
        {
            ARMARX_WARNING << "Segment start is NaN: " << VAROUT(segment.start)
                           << VAROUT(line.origin()) << VAROUT(line.direction());
        }
        if (anyIsNaN(segment.end))
        {
            ARMARX_WARNING << "Segment end is NaN: " << VAROUT(segment.end) << VAROUT(line.origin())
                           << VAROUT(line.direction());
        }
 
        return segment;
    }
 
    float
    leastSquareEdge(Eigen::Vector2f* begin, Eigen::Vector2f* end, LineSegment2Df& segment)
    {
        float maxError{0};
        float averageError{0};
        segment = leastSquareLineSegmentInOrder(begin, end, &maxError, &averageError);
 
        return averageError;
    }
} // namespace
 
LaserScannerSelfLocalisation::LaserScannerSelfLocalisation() :
    s(0.1) // This frequency will not be used
{
}
 
void
LaserScannerSelfLocalisation::onInitComponent()
{
    reportTopicName = getProperty<std::string>("ReportTopicName");
    robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
    platformName = getProperty<std::string>("PlatformName");
    laserScannerUnitName = getProperty<std::string>("LaserScannerUnitName");
    workingMemoryName = getProperty<std::string>("WorkingMemoryName");
    longtermMemoryName = getProperty<std::string>("LongtermMemoryName");
    mapFilename = getProperty<std::string>("MapFilename");
    agentName = getProperty<std::string>("AgentName");
    emergencyStopMasterName = getProperty<std::string>("EmergencyStopMasterName");
    debugObserverName = getProperty<std::string>("DebugObserverName");
    updateWorkingMemory = getProperty<bool>("UpdateWorkingMemory");
    int updatePeriodWorkingMemoryInMs = getProperty<int>("UpdatePeriodWorkingMemoryInMs");
    workingMemoryUpdateFrequency = 1000.0f / updatePeriodWorkingMemoryInMs;
    int updatePeriodLongtermMemory = getProperty<int>("UpdatePeriodLongtermMemoryInMs");
    longtermMemoryUpdateFrequency = 1000.0f / updatePeriodLongtermMemory;
    robotPositionZ = getProperty<float>("RobotPositionZ");
    maximalLaserScannerDelay = getProperty<float>("MaximalLaserScannerDelay").getValue();
 
    updateProperties(true);
 
    map = loadMapFromFile(mapFilename);
 
    {
        const auto min = getPropertyAsCSV<float>("PlatformRectangleMin");
        const auto max = getPropertyAsCSV<float>("PlatformRectangleMax");
        std::tie(platformRectMin(0), platformRectMax(0)) = std::minmax(min.at(0), max.at(0));
        std::tie(platformRectMin(1), platformRectMax(1)) = std::minmax(min.at(1), max.at(1));
        ARMARX_INFO << "using platform rectangle min / max " << platformRectMin.transpose() << " / "
                    << platformRectMax.transpose();
    }
 
 
    usingProxy(robotStateComponentName);
    usingProxy(laserScannerUnitName);
    usingProxy(workingMemoryName);
    usingProxy(longtermMemoryName);
}
 
void
LaserScannerSelfLocalisation::onConnectComponent()
{
    robotState = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
    laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
    SharedRobotInterfacePrx sharedRobot = robotState->getSynchronizedRobot();
    for (LaserScannerInfo const& info : laserScannerUnit->getConnectedDevices())
    {
        FramedPoseBasePtr basePose = sharedRobot->getRobotNode(info.frame)->getPoseInRootFrame();
        FramedPose& pose = dynamic_cast<FramedPose&>(*basePose);
 
        LaserScanData data;
        data.pose = pose.toEigen();
        data.info = info;
        data.mutex = std::make_shared<std::mutex>();
        scanData.push_back(data);
    }
 
    debugObserver = getTopic<DebugObserverInterfacePrx>(debugObserverName);
 
    robotRootFrame = sharedRobot->getRootNode()->getName();
 
    workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
    agentsMemory = workingMemory->getAgentInstancesSegment();
    if (agentName.empty())
    {
        agentName = robotState->getRobotName();
    }
    agentInstance = new memoryx::AgentInstance();
    agentInstance->setSharedRobot(sharedRobot);
    agentInstance->setAgentFilePath(robotState->getRobotFilename());
    agentInstance->setName(agentName);
 
    longtermMemory = getProxy<memoryx::LongtermMemoryInterfacePrx>(longtermMemoryName);
    selfLocalisationSegment = longtermMemory->getSelfLocalisationSegment();
 
    offeringTopic(reportTopicName);
    reportTopic = getTopic<LaserScannerSelfLocalisationListenerPrx>(reportTopicName);
    offeringTopic("PlatformState");
    platformUnitTopic = getTopic<PlatformUnitListenerPrx>("PlatformState");
    // Load estimatedPose from longterm memory
    estimatedPose = Eigen::Vector3f::Zero();
    memoryx::EntityBasePtr poseEntity = selfLocalisationSegment->getEntityByName(agentName);
    if (poseEntity)
    {
        poseEntityId = poseEntity->getId();
        ARMARX_INFO << "Using pose entity from LTM with ID: " << poseEntityId;
        memoryx::EntityAttributeBasePtr poseAttribute = poseEntity->getAttribute("pose");
        if (poseAttribute)
        {
            // So this is how you access a typed attribute, fancy
            FramedPosePtr framedPose = armarx::VariantPtr::dynamicCast(poseAttribute->getValueAt(0))
                                           ->getClass<armarx::FramedPose>();
            if (framedPose)
            {
                Eigen::Matrix4f globalPose = framedPose->toGlobalEigen(sharedRobot);
                Eigen::Matrix3f robotRot = globalPose.block<3, 3>(0, 0);
                Eigen::Vector2f robotPos = globalPose.block<2, 1>(0, 3);
                Eigen::Vector2f yWorld(0.0f, 1.0f);
                Eigen::Vector2f yRobot = robotRot.col(1).head<2>();
                float robotTheta = acos(yWorld.dot(yRobot));
                if (yRobot.x() >= 0.0f)
                {
                    robotTheta = -robotTheta;
                }
 
                estimatedPose = Eigen::Vector3f(robotPos.x(), robotPos.y(), robotTheta);
            }
            else
            {
                ARMARX_WARNING << "'pose' attribute does not contain a framed pose";
            }
        }
        else
        {
            ARMARX_WARNING << "No attribute 'pose' in entity for agent '" << agentName << "' found";
        }
    }
    else
    {
        ARMARX_WARNING << "No pose in longterm memory found for agent: " << agentName;
        ARMARX_WARNING << "Self localisation will start at origin";
 
        FramedPosePtr pose = new FramedPose(Eigen::Matrix4f::Identity(), GlobalFrame, "");
        memoryx::SimpleEntityPtr poseEntity = new memoryx::SimpleEntity();
        poseEntity->setName(agentName);
        poseEntity->putAttribute("pose", pose);
        //poseEntity->setId(poseEntityId);
 
        poseEntityId = selfLocalisationSegment->addEntity(poseEntity);
        ARMARX_INFO << "New entity ID: " << poseEntityId;
    }
    ARMARX_INFO << "Initial pose: (" << estimatedPose[0] << ", " << estimatedPose[1] << ", "
                << estimatedPose[2] << ")";
    resetKalmanFilter(estimatedPose);
    lastVelocityUpdate = TimeUtil::GetTime();
    lastVelocity = Eigen::Vector3f::Zero();
 
    usingTopic(laserScannerUnit->getReportTopicName());
    usingTopic(platformName + "State");
 
 
    int updatePeriod = getProperty<int>("UpdatePeriodInMs");
    task = new PeriodicTask<LaserScannerSelfLocalisation>(
        this,
        &LaserScannerSelfLocalisation::updateLocalisation,
        updatePeriod,
        "UpdateLocalisation");
    task->start();
}
 
void
LaserScannerSelfLocalisation::onDisconnectComponent()
{
    task->stop();
    ARMARX_INFO << "Task stopped";
    if (laserScannerFileLoggingData)
    {
        writeLogFile();
    }
}
 
void
LaserScannerSelfLocalisation::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
LaserScannerSelfLocalisation::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr prop =
        new LaserScannerSelfLocalisationPropertyDefinitions(getConfigIdentifier());
 
    prop->topic(globalRobotPoseTopic);
 
    return prop;
}
 
std::string
LaserScannerSelfLocalisation::getReportTopicName(const Ice::Current&)
{
    return reportTopicName;
}
 
void
LaserScannerSelfLocalisation::setAbsolutePose(Ice::Float x,
                                              Ice::Float y,
                                              Ice::Float theta,
                                              const Ice::Current&)
{
    {
        std::unique_lock lock(setPoseMutex);
        setPose = Eigen::Vector3f(x, y, theta);
        resetKalmanFilter(*setPose);
    }
}
 
void
LaserScannerSelfLocalisation::reportSensorValues(const std::string& device,
                                                 const std::string& name,
                                                 const LaserScan& scan,
                                                 const TimestampBasePtr& timestamp,
                                                 const Ice::Current&)
{
    for (LaserScanData& data : scanData)
    {
        if (data.info.device == device)
        {
            std::unique_lock lock(*data.mutex);
            data.scan = scan;
            data.measurementTime = IceUtil::Time::microSeconds(timestamp->timestamp);
        }
    }
}
 
static Eigen::Vector3f
addPose(Eigen::Vector3f pose, Eigen::Vector3f add)
{
    Eigen::Vector3f result = pose + add;
    // Make sure that theta is in the range [-PI,PI]
    result[2] = armarx::math::MathUtils::angleModPI(result[2]);
    return result;
}
 
static Eigen::Vector3f
integratePose(Eigen::Vector3f pose, Eigen::Vector3f v0, Eigen::Vector3f v1, float dt)
{
    // Assumption: Linear acceleration between v0 and v1 in dt seconds
    // Travelled distance d = v0*dt + 0.5*(v1-v0)*dt = 0.5*dt*(v0+v1)
 
    float dTheta = 0.5f * dt * (v0[2] + v1[2]);
    // This is an approximation (the right solution would have to integrate over changing theta)
    float thetaMid = pose[2] + 0.5f * dTheta;
    Eigen::Matrix2f rotation = Eigen::Rotation2Df(thetaMid).matrix();
    Eigen::Vector2f globalV0 = rotation * v0.head<2>();
    Eigen::Vector2f globalV1 = rotation * v1.head<2>();
    Eigen::Vector2f globalDeltaPos = 0.5f * dt * (globalV0 + globalV1);
 
    Eigen::Vector3f d(globalDeltaPos.x(), globalDeltaPos.y(), dTheta);
    return addPose(pose, d);
}
 
void
LaserScannerSelfLocalisation::reportPlatformVelocity(Ice::Float velX,
                                                     Ice::Float velY,
                                                     Ice::Float velTheta,
                                                     const Ice::Current&)
{
    IceUtil::Time now = TimeUtil::GetTime();
    auto elapsedSeconds = now - lastVelocityUpdate;
    lastVelocityUpdate = now;
    {
        std::unique_lock lock(kalmanMutex);
        filter->predict(velX, velY, velTheta, elapsedSeconds);
    }
    // Store odometry reports here and calculate the estimated pose in the central update task
    if (useOdometry)
    {
 
        std::unique_lock lock(odometryMutex);
 
        reportedVelocities.push_back(ReportedVelocity{
            static_cast<float>(elapsedSeconds.toSecondsDouble()), velX, velY, velTheta});
    }
}
 
void
LaserScannerSelfLocalisation::reportPlatformOdometryPose(Ice::Float,
                                                         Ice::Float,
                                                         Ice::Float,
                                                         const Ice::Current&)
{
    // nop
}
 
void
LaserScannerSelfLocalisation::updateLocalisation()
{
    // Periodic task to update the self localisation
 
    // Get the current parameter values;
    propertyMutex.lock();
    int halfFrameSize = propSmoothFrameSize / 2;
    int mergeDistance = propSmoothMergeDistance;
    float maxDistanceSquared = propMatchingMaxDistance * propMatchingMaxDistance;
    float minPointsForMatch = propMatchingMinPoints;
    float matchingCorrectionFactor = propMatchingCorrectionFactor;
    bool reportPoints = propReportPoints;
    bool reportEdges = propReportEdges;
    bool useMapCorrection = propUseMapCorrection;
    int epsilon = propEdgeEpsilon;
    int minPointsPerEdge = propEdgeMinPoints;
    float maxDistanceForEdgeSquared = propEdgeMaxDistance * propEdgeMaxDistance;
    float pointAddingThreshold = propEdgePointAddingThreshold;
    float edgeMaxDeltaAngle = propEdgeMaxDeltaAngle;
    propertyMutex.unlock();
 
    Eigen::Vector3f currentPose = estimatedPose;
 
    // Apply the collected odometry pose correction
    if (useOdometry)
    {
        std::unique_lock lock(odometryMutex);
        for (ReportedVelocity const& report : reportedVelocities)
        {
            Eigen::Vector3f currentVelocity(report.x, report.y, report.theta);
            Eigen::Vector3f motionPrediction =
                integratePose(currentPose, lastVelocity, currentVelocity, report.dt);
            if (anyIsNaN(motionPrediction))
            {
                ARMARX_WARNING << "Motion prediction is NaN: " << motionPrediction;
                ARMARX_WARNING << VAROUT(currentPose) << " " << VAROUT(lastVelocity) << " "
                               << VAROUT(currentVelocity) << " " << VAROUT(report.dt);
            }
            else
            {
                currentPose = motionPrediction;
            }
 
            lastVelocity = currentVelocity;
        }
        reportedVelocities.clear();
    }
 
    // Use the laser scan data to make a global pose correction
    std::size_t pointsSize = 0;
    IceUtil::Time now = TimeUtil::GetTime();
    IceUtil::Time measurementTime = IceUtil::Time::seconds(0);
    // No data available
    if (scanData.empty())
    {
        measurementTime = now;
    }
    for (LaserScanData const& data : scanData)
    {
        measurementTime = std::max(measurementTime, data.measurementTime);
 
        std::unique_lock lock(*data.mutex);
        if (data.scan.empty())
        {
            continue;
        }
 
        pointsSize += data.scan.size();
 
        // Check whether the scan data is out of date
        double deltaMeasurement = (now - data.measurementTime).toSecondsDouble();
        if (deltaMeasurement < -0.005)
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Delta between now and measurement time is negative: "
                           << deltaMeasurement << " now: " << now.toDateTime()
                           << " measurement: " << data.measurementTime.toDateTime();
        }
        else if (deltaMeasurement > maximalLaserScannerDelay)
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Delta between now and measurement time has exceeded the limit ("
                           << deltaMeasurement << "s > " << maximalLaserScannerDelay
                           << "s). Activating emergency stop!";
 
            EmergencyStopMasterInterfacePrx emergencyStopMaster =
                getProxy<EmergencyStopMasterInterfacePrx>(emergencyStopMasterName);
            if (emergencyStopMaster)
            {
                emergencyStopMaster->setEmergencyStopState(eEmergencyStopActive);
            }
            else
            {
                ARMARX_ERROR << "Could not activate emergency stop because proxy for "
                                "EmergencyStopMaster was not found '"
                             << emergencyStopMasterName << "'";
            }
        }
    }
    if (useMapCorrection && pointsSize > 0)
    {
        for (LaserScanData& data : scanData)
        {
            Eigen::Rotation2Df globalRotation = Eigen::Rotation2Df(currentPose[2]);
            Eigen::Vector2f globalTranslation = currentPose.head<2>();
 
            std::unique_lock lock(*data.mutex);
            data.points.clear();
            data.points.reserve(data.scan.size());
            for (int stepIndex = 0; stepIndex < (int)data.scan.size(); ++stepIndex)
            {
                LaserScanStep const& step = data.scan[stepIndex];
 
                // Smoothing in a frame neighborhood
                float distanceSum = 0;
                int distanceElements = 0;
                int minFrameIndex = std::max(0, stepIndex - halfFrameSize);
                int maxFrameIndex =
                    std::min((int)(data.scan.size()) - 1, stepIndex + halfFrameSize);
                for (int frameIndex = minFrameIndex; frameIndex <= maxFrameIndex; ++frameIndex)
                {
                    float frameDistance = data.scan[frameIndex].distance;
                    int deltaDistance = std::abs(step.distance - frameDistance);
                    if (deltaDistance <= mergeDistance)
                    {
                        distanceSum += frameDistance;
                        ++distanceElements;
                    }
                }
                if (distanceElements == 0)
                {
                    ARMARX_WARNING
                        << "No elements with distance smaller than merge distance found.";
                    ARMARX_WARNING << VAROUT(stepIndex) << " " << VAROUT(step.distance) << " "
                                   << VAROUT(mergeDistance);
                }
                else
                {
                    float distance = distanceSum / distanceElements;
 
                    // Transform to global 2D point
                    float sinAngle, cosAngle;
                    sincosf(step.angle, &sinAngle, &cosAngle);
                    float x = -distance * sinAngle;
                    float y = distance * cosAngle;
 
                    Eigen::Vector2f localPoint =
                        (data.pose * Eigen::Vector4f(x, y, 0, 1)).head<2>();
 
                    if (anyIsNaN(localPoint))
                    {
                        ARMARX_WARNING << "Local point is NaN";
                        ARMARX_WARNING << VAROUT(localPoint) << " " << VAROUT(x) << " " << VAROUT(y)
                                       << " " << VAROUT(step.angle);
                        continue;
                    }
 
                    if (platformRectMin(0) < localPoint(0) && localPoint(0) < platformRectMax(0) &&
                        platformRectMin(1) < localPoint(1) && localPoint(1) < platformRectMax(1))
                    {
                        ARMARX_INFO << deactivateSpam()
                                    << "discarding point inside of the platform rectangle: "
                                    << localPoint.transpose();
                        continue;
                    }
 
                    Eigen::Vector2f globalPoint = globalRotation * localPoint + globalTranslation;
 
                    if (anyIsNaN(globalPoint))
                    {
                        ARMARX_WARNING << "Global point is NaN";
                        ARMARX_WARNING << VAROUT(globalPoint) << " " << VAROUT(x) << " "
                                       << VAROUT(y) << " " << VAROUT(step.angle);
                    }
                    else
                    {
                        data.points.push_back(globalPoint);
                    }
                }
            }
            ARMARX_DEBUG << deactivateSpam() << "using " << data.points.size() << "/"
                         << data.scan.size();
        }
        if (reportPoints)
        {
            std::vector<armarx::Vector2f> pointsA;
            pointsA.reserve(pointsSize);
            for (LaserScanData const& data : scanData)
            {
                for (auto& p : data.points)
                {
                    pointsA.push_back(armarx::Vector2f{p.x(), p.y()});
                }
            }
            if (reportTopic)
            {
                reportTopic->begin_reportLaserScanPoints(pointsA);
            }
            else
            {
                ARMARX_WARNING << "Report topic is not set";
            }
        }
 
        std::size_t edgeCount = 0;
 
        for (LaserScanData& data : scanData)
        {
            auto& points = data.points;
 
            std::vector<float> angles;
            angles.reserve(points.size());
            int pointsSize = (int)points.size();
            for (int pointIndex = 0; pointIndex < pointsSize; ++pointIndex)
            {
                int beginIndex = std::max(pointIndex - epsilon, 0);
                int endIndex = std::min(pointIndex + epsilon + 1, pointsSize - 1);
                Eigen::Vector2f* beginPoint = &data.points[beginIndex];
                Eigen::Vector2f* endPoint = &data.points[endIndex];
                Line line(leastSquareLine(beginPoint, endPoint));
                float angle = std::atan2(line.direction().x(), line.direction().y());
                if (std::isnan(angle))
                {
                    ARMARX_VERBOSE << "Edge angle is NaN" << VAROUT(beginIndex) << " "
                                   << VAROUT(endIndex);
                }
                else
                {
                    angles.push_back(angle);
                }
            }
 
            int beginIndex = 0;
            int endIndex = beginIndex;
            int maxEndIndex = pointsSize - 1;
            int minBeginIndex = 0;
 
            int maximumPointsInEdge = 0;
            LineSegment2Df edgeLineSegment[2];
            int resultIndex = 0;
            data.edges.clear();
            while (endIndex < maxEndIndex)
            {
                float yaw = angles[endIndex];
 
                // We compare the angles of following points
                // If they stay within parametrized limits (angle and distance),
                // we add them to the edge
                while (endIndex < maxEndIndex &&
                       std::fabs(yaw - angles[endIndex + 1]) < edgeMaxDeltaAngle &&
                       (points[endIndex + 1] - points[endIndex]).squaredNorm() <
                           maxDistanceForEdgeSquared)
                {
                    endIndex++;
                }
 
                // Corners have different angles but still are part of an edge
                // Therefore we must extend both ends of the edge
                // The extension continues until the least squares error exceeds a parameterized limit.
 
                bool frontExtended = false;
                bool backExtended = false;
 
                // Extend the front
                while (beginIndex > minBeginIndex &&
                       (points[beginIndex - 1] - points[beginIndex]).squaredNorm() <
                           maxDistanceForEdgeSquared &&
                       leastSquareEdge(points.data() + beginIndex - 1,
                                       points.data() + endIndex + 1,
                                       edgeLineSegment[resultIndex]) < pointAddingThreshold)
                {
                    beginIndex--;
                    resultIndex = !resultIndex;
                    frontExtended = true;
                }
 
                // Extend the back
                while (endIndex < maxEndIndex &&
                       (points[endIndex] - points[endIndex + 1]).squaredNorm() <
                           maxDistanceForEdgeSquared &&
                       leastSquareEdge(points.data() + beginIndex,
                                       points.data() + endIndex + 1,
                                       edgeLineSegment[resultIndex]) < pointAddingThreshold)
                {
                    endIndex++;
                    resultIndex = !resultIndex;
                    backExtended = true;
                }
 
                if (!frontExtended && !backExtended)
                {
                    leastSquareEdge(points.data() + beginIndex,
                                    points.data() + endIndex + 1,
                                    edgeLineSegment[!resultIndex]);
                }
 
                // Check for minimum number of points in the edge
                int numberOfPoints = endIndex - beginIndex + 1;
                if (numberOfPoints > minPointsPerEdge)
                {
                    // Get the extended line segment which did not exceed the error limit
                    // Index = resultIndex: contains the line segment which exceeded the error limit
                    LineSegment2Df& segment = edgeLineSegment[!resultIndex];
                    Eigen::Vector2f* pointsBegin = points.data() + beginIndex;
                    ExtractedEdge edge{segment, pointsBegin, pointsBegin + numberOfPoints};
                    data.edges.push_back(edge);
                    ++edgeCount;
                    maximumPointsInEdge = std::max(numberOfPoints, maximumPointsInEdge);
                    minBeginIndex = endIndex + 1;
                }
 
                // Search for the next edge
                beginIndex = endIndex + 1;
                endIndex = beginIndex;
            }
        }
 
        if (reportEdges)
        {
            std::vector<armarx::LineSegment2D> segments;
            segments.reserve(edgeCount);
            for (LaserScanData const& data : scanData)
            {
                for (ExtractedEdge const& e : data.edges)
                {
                    armarx::LineSegment2D s;
                    s.start = armarx::Vector2f{e.segment.start.x(), e.segment.start.y()};
                    s.end = armarx::Vector2f{e.segment.end.x(), e.segment.end.y()};
                    segments.push_back(s);
                }
            }
            reportTopic->begin_reportExtractedEdges(segments);
        }
 
        std::size_t rowCount = 0;
        Eigen::Vector2f robotPos = currentPose.head<2>();
 
        Eigen::MatrixX3f X(pointsSize, 3);
        Eigen::VectorXf Y(pointsSize);
        for (LaserScanData const& data : scanData)
        {
            for (ExtractedEdge const& edge : data.edges)
            {
                for (auto* p = edge.pointsBegin; p != edge.pointsEnd; ++p)
                {
                    auto& globalPoint = *p;
                    // Find the line segment closest to this point
                    LineSegment2Df* target = nullptr;
                    float minDistanceSquared = FLT_MAX;
                    for (LineSegment2Df& segment : map)
                    {
                        float distanceSquared =
                            lineSegmentToPointDistanceSquared(segment, globalPoint);
                        if (distanceSquared < minDistanceSquared)
                        {
                            minDistanceSquared = distanceSquared;
                            target = &segment;
                        }
                    }
                    if (minDistanceSquared > maxDistanceSquared)
                    {
                        continue;
                    }
                    if (!target)
                    {
                        ARMARX_WARNING << "No line segment closest to point found: " << globalPoint;
                        continue;
                    }
 
                    Eigen::Vector2f const& v = globalPoint;
                    Eigen::Vector2f targetDir = target->end - target->start;
                    Eigen::Vector2f u = Eigen::Vector2f(-targetDir.y(), targetDir.x()).normalized();
                    float r = target->start.dot(u);
                    Eigen::Vector2f cToV(v - robotPos);
                    Eigen::Vector2f cToVOrth(-cToV.y(), cToV.x());
                    float x3 = u.dot(cToVOrth);
                    float y = r - u.dot(v);
 
                    X(rowCount, 0) = u(0);
                    X(rowCount, 1) = u(1);
                    X(rowCount, 2) = x3;
                    Y(rowCount) = y;
                    ++rowCount;
                }
            }
        }
 
        Eigen::Vector3f b = Eigen::Vector3f::Zero();
        float matched = static_cast<float>(rowCount) / pointsSize;
        debugObserver->setDebugDatafield(getName(), "MatchedDataPointRatio", new Variant(matched));
        if (matched < minPointsForMatch)
        {
            ARMARX_WARNING
                << deactivateSpam(5)
                << "Too few data points could be associated with edges of the map (associated: "
                << rowCount << "/" << pointsSize << " = " << matched << ")";
        }
        else if (rowCount > 10)
        {
            b = X.block(0, 0, rowCount, 3)
                    .jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
                    .solve(Y.head(rowCount));
            b *= matchingCorrectionFactor;
            if (anyIsNaN(b))
            {
                ARMARX_WARNING << deactivateSpam(1) << "Equation solution is NaN " << VAROUT(b)
                               << " " << VAROUT(rowCount);
            }
        }
 
        Eigen::Vector3f newEstimatedPose = addPose(currentPose, b);
        if (anyIsNaN(newEstimatedPose))
        {
            ARMARX_WARNING << "New estimated pose is NaN " << VAROUT(currentPose) << " "
                           << VAROUT(b);
        }
        else
        {
            estimatedPose = newEstimatedPose;
        }
        {
            std::unique_lock lock(kalmanMutex);
            if (propSensorStdDev > 0)
            {
                filter->update(estimatedPose(0), estimatedPose(1), estimatedPose(2));
            }
        }
    }
    else
    {
        estimatedPose = currentPose;
    }
    //    ARMARX_INFO << deactivateSpam(1) << VAROUT(estimatedPose) << VAROUT(propSensorStdDev);
    Eigen::Vector3f filteredPose;
    if (propSensorStdDev > 0)
    {
        Eigen::Matrix3d cov = filter->getCovariance();
        reportTopic->begin_reportPoseUncertainty(cov(0, 0), cov(1, 1), cov(2, 2));
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(cov);
        filteredPose = filter->getPose().cast<float>();
    }
    else
    {
        filteredPose = estimatedPose;
    }
    {
        std::unique_lock lock(setPoseMutex);
        if (setPose)
        {
            estimatedPose = *setPose;
            resetKalmanFilter(estimatedPose);
            setPose.reset();
        }
    }
    reportTopic->begin_reportCorrectedPose(filteredPose[0], filteredPose[1], filteredPose[2]);
    PlatformPose newPose;
    newPose.timestampInMicroSeconds = measurementTime.toMicroSeconds();
    newPose.x = filteredPose[0];
    newPose.y = filteredPose[1];
    newPose.rotationAroundZ = filteredPose[2];
    // platformUnitTopic->reportPlatformPose(newPose);
 
    Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity();
    orientation.block<2, 2>(0, 0) = Eigen::Rotation2Df(filteredPose[2]).matrix();
    Eigen::Vector3f position(filteredPose[0], filteredPose[1], robotPositionZ);
    FramedPosePtr reportPose = new FramedPose(orientation, position, GlobalFrame, "");
 
 
    FrameHeader header;
    header.parentFrame = GlobalFrame;
    header.frame = robotRootFrame;
    header.agent = agentName;
    header.timestampInMicroSeconds = measurementTime.toMicroSeconds();
 
    TransformStamped globalRobotPose;
    globalRobotPose.header = header;
    globalRobotPose.transform = reportPose->toEigen();
 
    globalRobotPoseTopic->reportGlobalRobotPose(globalRobotPose);
 
    try
    {
        if (updateWorkingMemory &&
            s.checkFrequency("workingMemoryUpdate", workingMemoryUpdateFrequency))
        {
            // This seems to be the way to update the current pose
            // Since in simulation another component (SelfLocalizationDynamicSimulation) updates this memory location
            // it is a bad idea to enable the working memory update in simulation
            if (agentInstance)
            {
                agentInstance->setPose(reportPose);
                if (agentsMemory)
                {
                    std::string robotAgentId =
                        agentsMemory->upsertEntityByName(agentInstance->getName(), agentInstance);
                    agentInstance->setId(robotAgentId);
                }
                else
                {
                    ARMARX_WARNING << "Agents memory not set";
                }
            }
            else
            {
                ARMARX_WARNING << "Agent instance not set";
            }
        }
 
        if (s.checkFrequency("longtermMemoryUpdate", longtermMemoryUpdateFrequency))
        {
            memoryx::SimpleEntityPtr poseEntity = new memoryx::SimpleEntity();
            poseEntity->setName(agentName);
            poseEntity->putAttribute("pose", reportPose);
            poseEntity->setId(poseEntityId);
            // We just update the 'pose' attribute since ID and name have been set by the inital insert
            if (selfLocalisationSegment)
            {
                selfLocalisationSegment->upsertEntity(poseEntityId, poseEntity);
            }
            else
            {
                ARMARX_WARNING << "Self localisation memory segment is not set";
            }
        }
    }
    catch (IceUtil::Exception const& ex)
    {
        ARMARX_WARNING << "Caught exception while updating the memory:\n" << ex;
    }
 
    if (laserScannerFileLoggingData)
    {
        laserScannerFileLoggingData->scanDataHistory[now] = (scanData);
    }
}
 
void
LaserScannerSelfLocalisation::updateProperties(bool initial)
{
#define ARMARX_LSSL_UPDATE_PROPERTY(name)                                                          \
    {                                                                                              \
        auto prop = getProperty<decltype(prop##name)>(#name);                                      \
        if (initial || prop.isSet())                                                               \
        {                                                                                          \
            prop##name = prop.getValue();                                                          \
            ARMARX_VERBOSE << VAROUT(prop##name);                                                  \
        }                                                                                          \
    }
    try
    {
        ARMARX_VERBOSE << "Updating properties:";
        std::unique_lock lock(propertyMutex);
 
 
        if (std::abs(propSensorStdDev - getProperty<float>("SensorStdDev").getValue()) > 0.001)
        {
            propSensorStdDev = getProperty<float>("SensorStdDev");
            ARMARX_VERBOSE << "new " << VAROUT(propSensorStdDev);
            if (!initial)
            {
                resetKalmanFilter(estimatedPose);
            }
        }
        if (std::abs(propVelSensorStdDev - getProperty<float>("VelSensorStdDev").getValue()) >
            0.00001)
        {
            propVelSensorStdDev = getProperty<float>("VelSensorStdDev");
            ARMARX_VERBOSE << "new " << VAROUT(propVelSensorStdDev);
            if (!initial)
            {
                resetKalmanFilter(estimatedPose);
            }
        }
 
 
        ARMARX_LSSL_UPDATE_PROPERTY(SmoothFrameSize);
        ARMARX_LSSL_UPDATE_PROPERTY(SmoothMergeDistance);
        ARMARX_LSSL_UPDATE_PROPERTY(MatchingMaxDistance);
        ARMARX_LSSL_UPDATE_PROPERTY(MatchingMinPoints);
        ARMARX_LSSL_UPDATE_PROPERTY(MatchingCorrectionFactor);
        ARMARX_LSSL_UPDATE_PROPERTY(EdgeMaxDistance);
        ARMARX_LSSL_UPDATE_PROPERTY(EdgeMaxDeltaAngle);
        ARMARX_LSSL_UPDATE_PROPERTY(EdgePointAddingThreshold);
        ARMARX_LSSL_UPDATE_PROPERTY(EdgeEpsilon);
        ARMARX_LSSL_UPDATE_PROPERTY(EdgeMinPoints);
        ARMARX_LSSL_UPDATE_PROPERTY(UseOdometry);
        ARMARX_LSSL_UPDATE_PROPERTY(UseMapCorrection);
        ARMARX_LSSL_UPDATE_PROPERTY(ReportPoints);
        ARMARX_LSSL_UPDATE_PROPERTY(ReportEdges);
        ARMARX_LSSL_UPDATE_PROPERTY(LoggingFilePath);
        //        ARMARX_LSSL_UPDATE_PROPERTY(SensorStdDev);
        //        ARMARX_LSSL_UPDATE_PROPERTY(VelSensorStdDev);
 
 
        if (!propLoggingFilePath.empty())
        {
            if (laserScannerFileLoggingData && propRecordData &&
                !getProperty<bool>("RecordData").getValue())
            {
                writeLogFile();
            }
            if (!propRecordData && getProperty<bool>("RecordData").getValue())
            {
                // setup data
                laserScannerFileLoggingData.reset(new LaserScannerFileLoggingData());
                laserScannerFileLoggingData->filePath = propLoggingFilePath;
            }
            propRecordData = getProperty<bool>("RecordData").getValue();
        }
 
        useOdometry = propUseOdometry;
    }
    catch (std::exception const& ex)
    {
        ARMARX_WARNING << "Could not update properties. " << ex.what();
    }
 
#undef ARMARX_LSSL_UPDATE_PROPERTY
}
 
void
LaserScannerSelfLocalisation::resetKalmanFilter(const Eigen::Vector3f& pose)
{
    ARMARX_INFO << "Position Std Dev: " << propSensorStdDev
                << " Vel Std Dev: " << propVelSensorStdDev;
    std::unique_lock lock(kalmanMutex);
    filter.reset(new memoryx::PlatformKalmanFilter(
        pose.head<2>().cast<double>(),
        pose(2),
        propSensorStdDev,
        propSensorStdDev / 100,
        propVelSensorStdDev,
        propVelSensorStdDev /* no scaling needed since motion noise scaling is done with rad/s*/));
}
 
void
LaserScannerSelfLocalisation::writeLogFile()
{
    if (!laserScannerFileLoggingData)
    {
        ARMARX_INFO << deactivateSpam(1)
                    << "laserScannerFileLoggingData is NULL - cannot write log file";
        return;
    }
    std::map<IceUtil::Time, std::vector<LaserScanData>> dataMap;
    std::string filePath;
    {
        std::unique_lock lock(laserScannerFileLoggingData->loggingMutex);
        dataMap.swap(laserScannerFileLoggingData->scanDataHistory);
        filePath = laserScannerFileLoggingData->filePath;
    }
    ARMARX_INFO << "Writing " << dataMap.size() << " laser scans";
    laserScannerFileLoggingData.reset();
    nlohmann::json json;
    std::ofstream file(filePath, std::ofstream::out | std::ofstream::trunc);
    ARMARX_CHECK_EXPRESSION(file.is_open()) << filePath;
    //                if(!dataMap.empty() && !dataMap.begin()->second.empty())
    //                {
    //                    auto & LaserScanData = dataMap.begin()->second.empty();
    //                }
    int skipLineAmount = 10;
    int i = 0;
    for (auto& pair : dataMap)
    {
        if (i % skipLineAmount != 0)
        {
            continue;
        }
        const std::vector<LaserScanData>& scanData = pair.second;
        for (auto& elem : scanData)
        {
            const LaserScanData& scan = elem;
            Eigen::Quaternionf quat(scan.pose.block<3, 3>(0, 0));
            json["DeviceInfo"][scan.info.device] = {
                {"frame", scan.info.frame},
                {"minAngle", scan.info.minAngle},
                {"maxAngle", scan.info.maxAngle},
                {"stepSize", scan.info.stepSize},
                {"sensorPosition",
                 {{"x", scan.pose(0, 3)}, {"y", scan.pose(1, 3)}, {"z", scan.pose(2, 3)}}},
                {"sensorOrientation",
                 {{"w", quat.w()}, {"x", quat.x()}, {"y", quat.y()}, {"z", quat.z()}}}
 
            };
            Ice::IntSeq dataPts;
            for (const auto& pt : scan.points)
            {
                dataPts.push_back(pt(0));
                dataPts.push_back(pt(1));
            }
            json["Data"][scan.info.device]["GlobalPoints"] += dataPts;
            Ice::IntSeq scanPts;
            for (const LaserScanStep& pt : scan.scan)
            {
                scanPts.push_back(pt.distance);
            }
            json["Data"][scan.info.device]["ScanDistances"] += scanPts;
        }
        i++;
    }
    file << std::setw(1) << json << std::endl;
}
 
LineSegment2DSeq
LaserScannerSelfLocalisation::getMap(const Ice::Current&)
{
    LineSegment2DSeq result;
    for (LineSegment2Df const& segment : map)
    {
        armarx::Vector2f start{segment.start.x(), segment.start.y()};
        armarx::Vector2f end{segment.end.x(), segment.end.y()};
        result.push_back(LineSegment2D{start, end});
    }
    return result;
}
 
void
LaserScannerSelfLocalisation::componentPropertiesUpdated(
    const std::set<std::string>& changedProperties)
{
    updateProperties();
}