#include "main_algorithm.h"

Main_Algorithm::Main_Algorithm(QObject *parent)
    :QThread(parent) {}

Main_Algorithm::~Main_Algorithm(){}

void Main_Algorithm::takeFrames(cv::Mat& front_frame, cv::Mat& side_frame)
{
    if(this->format==-99) throw std::runtime_error("Pixel format NOT setted");
    if(this->duration==0) throw std::runtime_error("Captuuring WatchDog NOT setted");
    bool skip = false;
    std::future<camera::frame> futF_ = std::async(std::launch::async, [this]() { return front_camera->readImage(this->format,-1,true); });
    std::future<camera::frame> futS_ = std::async(std::launch::async, [this]() { return side_camera->readImage(this->format,-1,true); });
    if(futF_.wait_for(std::chrono::microseconds(long(duration)))==std::future_status::timeout)
       skip = true;
    if(futS_.wait_for(std::chrono::microseconds(long(duration)))==std::future_status::timeout)
        skip = true;

    front_frame = futF_.get().image;
    side_frame = futS_.get().image;
}

ri::Point Main_Algorithm::estimateRobotPosition(cv::Point frontTCP, cv::Point sideTCP, bool& flag)
{
   // QMessageBox msg;
    cv::Point3i diff;
    ri::Point estimated_position = this->robot->getPosition();
    std::ofstream myfile("/home/david/example.txt");

    tcpc::getPxOffset(this->settings.tcpc.calibration_Point.front,frontTCP,diff.y,'x');
    tcpc::getPxOffset(this->settings.tcpc.calibration_Point.side,sideTCP,diff.x,'x');
    tcpc::getPxOffset(this->settings.tcpc.calibration_Point.side,sideTCP,diff.z,'y');

    if(abs(diff.x)<=this->settings.tcpc.precision_px &&
       abs(diff.y)<=this->settings.tcpc.precision_px &&
       abs(diff.z)<=this->settings.tcpc.precision_px){
//        msg.setText("Position reached");
//        msg.exec();
        flag = true;
    }else{
        flag = false;
        estimated_position.x -= (diff.x*this->settings.camera_robot.coordinates_offset.x);
        estimated_position.y -= (diff.y*this->settings.camera_robot.coordinates_offset.y);
        estimated_position.z -= (diff.z*this->settings.camera_robot.coordinates_offset.z);

        //emit stats
        std::stringstream ss;
        ss<<estimated_position;
        emit ShowStats(ss.str().c_str(),QString("x: "+QString::number(diff.x*this->settings.camera_robot.coordinates_offset.x)+
                                           " mm, y: "+QString::number(diff.x*this->settings.camera_robot.coordinates_offset.x)+
                                           " mm, z: "+QString::number(diff.z*this->settings.camera_robot.coordinates_offset.z)+" mm"));

    }
    myfile<<"X: "<<diff.x<<" Y: "<<diff.y<<" Z: "<<diff.z;
    myfile.close();
    return  estimated_position;
}

void Main_Algorithm::MarkFrame(cv::Mat& frame, cv::Point position, cv::Point req_position){
    cv::drawMarker(frame, req_position,  cv::Scalar(0, 255, 0), cv::MARKER_CROSS, 40, 10);
    cv::drawMarker(frame, position,  cv::Scalar(0, 0, 255), cv::MARKER_CROSS, 40, 10);
}

void Main_Algorithm::emitMarkedFrames(cv::Mat front_frame, cv::Point front_TCP, cv::Mat side_frame, cv::Point side_TCP)
{
    //Mark frames and show it
    MarkFrame(front_frame,front_TCP,this->settings.tcpc.calibration_Point.front);
    MarkFrame(side_frame,side_TCP,this->settings.tcpc.calibration_Point.side);

    //Convert from cv::Mat to QImage
    QImage front_image, side_image;
    QString front_tmp_path = "/tmp/frameF.png";
    QString side_tmp_path = "/tmp/frameS.png";
    cv::imwrite(front_tmp_path.toStdString(),front_frame);
    cv::imwrite(side_tmp_path.toStdString(),side_frame);
    front_image.load(front_tmp_path);
    side_image.load(side_tmp_path);

    //Emit frames
    emit ShowMarked(front_image,side_image);
}

void Main_Algorithm::run()
{
/*********************************************INITS*********************************************/
    cv::Point front_TCP, side_TCP;
    ri::Point estimated_Position;
    bool breakFLAG = false;
    uint16_t iter;
    std::vector<ri::Point> TCP_reached_points;
    QMutex mux;

    emit MeasurementProgressChanged(0.0);
    emit CalculationProgressChanged(0.0);

    this->stopFlag = false;
    if(this->isRunnable()){
        this->format = this->front_camera->get_Format_px();
        duration = long(this->front_camera->get_Exposure_us()) *10;
    }else throw std::runtime_error("Not possible to start TCP Calibration check setted parameters");


        cv::Mat front_frame, side_frame;
        this->takeFrames(front_frame,side_frame);
        QImage front_image, side_image;
        QString front_tmp_path = "/tmp/frameF.png";
        QString side_tmp_path = "/tmp/frameS.png";
        cv::imwrite(front_tmp_path.toStdString(),front_frame);
        cv::imwrite(side_tmp_path.toStdString(),side_frame);
        front_image.load(front_tmp_path);
        side_image.load(side_tmp_path);
        emit ShowMarked(front_image,side_image);
/******************************************1st_Position*****************************************/
    iter = 0;
    emit MeasurementProgressChanged(0.0);
    this->robot->setPositionJ(this->settings.tcpc.default_tcpc_positions.at(0));
    do {
        try {
            //Acquisition and processing
            processing_1st_position(front_TCP,side_TCP);
        } catch (...) {
            std::this_thread::sleep_for(std::chrono::seconds(1));

            /************************/
            mux.lock();
            if(this->stopFlag) return;
            mux.unlock();
            /************************/

            //Acquisition and processing
            processing_1st_position(front_TCP,side_TCP);
        }

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

        //Estimate new position
        estimated_Position = estimateRobotPosition(front_TCP, side_TCP, breakFLAG);

        //Check if possition is achievable
        if(breakFLAG){
            TCP_reached_points.push_back(estimated_Position);
            emit MeasurementProgressChanged(100.0*(1.0/3.0));
            break;
        }

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

        //Go to the New position
        this->robot->setPosition(estimated_Position);
        emit MeasurementProgressChanged((double(iter)/double(this->settings.tcpc.MAX_FINDING_ITER))*(100.0/3.0));
        iter++;

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

    } while (iter<this->settings.tcpc.MAX_FINDING_ITER);

    if(iter>=this->settings.tcpc.MAX_FINDING_ITER) throw std::runtime_error("NOT possible to reach estimated position");

/******************************************2nd_Position*****************************************/
    iter = 0;
    emit MeasurementProgressChanged(100.0*(1.0/3.0));
    this->robot->setPositionJ(this->settings.tcpc.default_tcpc_positions.at(1));
    do {
        try {
            //Acquisition and processing
            processing_2nd_position(front_TCP,side_TCP);
        } catch (...) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
            //Acquisition and processing

            /************************/
            mux.lock();
            if(this->stopFlag) return;
            mux.unlock();
            /************************/

            processing_2nd_position(front_TCP,side_TCP);
        }

        //Estimate new position
        estimated_Position = estimateRobotPosition(front_TCP, side_TCP, breakFLAG);

        //Check if possition is achievable
        if(breakFLAG){
            TCP_reached_points.push_back(estimated_Position);
            emit MeasurementProgressChanged(100.0*(2.0/3.0));
            break;
        }

        //Go to the New position
        this->robot->setPosition(estimated_Position);
        emit MeasurementProgressChanged((double(iter)/double(this->settings.tcpc.MAX_FINDING_ITER))*(100.0/3.0)+(100.0/3.0));
        iter++;
    } while (iter<this->settings.tcpc.MAX_FINDING_ITER);

    if(iter>=this->settings.tcpc.MAX_FINDING_ITER) throw std::runtime_error("NOT possible to reach estimated position");

/******************************************3rd_Position*****************************************/
    iter = 0;
    emit MeasurementProgressChanged(100.0*(2.0/3.0));
    this->robot->setPositionJ(this->settings.tcpc.default_tcpc_positions.at(2));
    do {
        try {
            //Acquisition and processing
            processing_3rd_position(front_TCP,side_TCP);
        } catch (...) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
            //Acquisition and processing
            processing_3rd_position(front_TCP,side_TCP);
        }

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

        //Estimate new position
        estimated_Position = estimateRobotPosition(front_TCP, side_TCP, breakFLAG);

        //Check if possition is achievable
        if(breakFLAG){
            TCP_reached_points.push_back(estimated_Position);
            emit MeasurementProgressChanged(100.0);
            break;
        }

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

        //Go to the New position
        this->robot->setPosition(estimated_Position);
        emit MeasurementProgressChanged((double(iter)/double(this->settings.tcpc.MAX_FINDING_ITER))*(100.0/3.0)+(200.0/3.0));
        iter++;

        /************************/
        mux.lock();
        if(this->stopFlag) return;
        mux.unlock();
        /************************/

    } while (iter<this->settings.tcpc.MAX_FINDING_ITER);

    if(iter>=this->settings.tcpc.MAX_FINDING_ITER) throw std::runtime_error("NOT possible to reach estimated position");

/******************************************Estimate_TCP*****************************************/
robot->goHome();

/************************/
mux.lock();
if(this->stopFlag) return;
mux.unlock();
/************************/

emit CalculationProgressChanged(0.0);
this->TCP = estimateTCP(TCP_reached_points);

/************************/
mux.lock();
if(this->stopFlag) return;
mux.unlock();
/************************/

emit CalculationProgressChanged(100.0);
emit NumberChanged(4);
}

void Main_Algorithm::stop()
{
    this->stopFlag = true;
}

void Main_Algorithm::getTCP(ri::Point& point)
{
    point = this->TCP;
}

void Main_Algorithm::setFront_camera(interfaceBALLUFF::Camera &camera)
{
    this->front_camera = &camera;
}

void Main_Algorithm::setSide_camera(interfaceBALLUFF::Camera& camera)
{
    this->side_camera = &camera;
}

bool Main_Algorithm::isRunnable()
{
    bool result = false;
    result = this->robot->isEstablished();
    //check default position >robotToDefaultPosition();
    result |= this->front_camera != nullptr && this->side_camera != nullptr;
    result |= detectGripper();
    result |= (this->settings.camera_robot.coordinates_offset.x==0) ||
              (this->settings.camera_robot.coordinates_offset.y==0) ||
              (this->settings.camera_robot.coordinates_offset.z==0);
    return result;
}

void Main_Algorithm::setRobot(ri::Staubli& robot)
{
    this->robot = &robot;
}

void Main_Algorithm::setSettings(tcpc::sParameters parameters)
{
    this->settings = parameters;
}

void Main_Algorithm::robotToDefaultPosition(){
    this->robot->setPosition(this->settings.Robot.default_position);
}

bool Main_Algorithm::detectGripper()
{
    cv::Mat front_frame = front_camera->readImage(front_camera->get_Format_px(),-1,true).image;
    cv::Mat side_frame = side_camera->readImage(side_camera->get_Format_px(),-1,true).image;
    try {

        //detect Contours
        std::vector<cv::Point> frontSideContours = tcpc::getSignificantContours(front_frame, settings.HSV_filter.front, settings.cv.Canny_threshold,
                                                                                settings.cv.Dilate_thickness);

        std::vector<cv::Point> rightSideContours = tcpc::getSignificantContours(side_frame, settings.HSV_filter.side, settings.cv.Canny_threshold,
                                                                                settings.cv.Dilate_thickness);
       //get TCP (sceneI)
        tcpc::getHighestPoint(frontSideContours,'y');
        tcpc::getHighestMiddle(rightSideContours,'y');
    } catch (...) {
        return false;
    }
    return true;
}

void Main_Algorithm::processing_1st_position(cv::Point& frontTCP, cv::Point& sideTCP)
{
    cv::Mat front_frame, side_frame;
    //Get frames
    this->takeFrames(front_frame,side_frame);

    //Detect contours
    std::vector<cv::Point> frontContours = tcpc::getSignificantContours(front_frame, settings.HSV_filter.front, settings.cv.Canny_threshold,
                                                                                         settings.cv.Dilate_thickness);

    std::vector<cv::Point> sideeContours = tcpc::getSignificantContours(side_frame, settings.HSV_filter.side, settings.cv.Canny_threshold,
                                                                                        settings.cv.Dilate_thickness);

    //Detect TCP in frames
    //tcpc::filterPointsbyGravity(sideeContours,">y");
    frontTCP = tcpc::getHighestMiddle(frontContours,'y');
    sideTCP = tcpc::getHighestMiddle(sideeContours,'y');

    //Emit Marked frames to UI
    emitMarkedFrames(front_frame, frontTCP, side_frame, sideTCP);
}

void Main_Algorithm::processing_2nd_position(cv::Point& frontTCP, cv::Point& sideTCP)
{
     cv::Mat front_frame, side_frame;
    //Get frames
    this->takeFrames(front_frame,side_frame);

    //Detect contours
    std::vector<cv::Point> frontContours = tcpc::getSignificantContours(front_frame, settings.HSV_filter.front, settings.cv.Canny_threshold,
                                                                                               settings.cv.Dilate_thickness);
    std::vector<cv::Point> sideeContours = tcpc::getSignificantContours(side_frame, settings.HSV_filter.side, settings.cv.Canny_threshold,
                                                                                        settings.cv.Dilate_thickness);

    //Detect TCP in frames
    tcpc::filterPointsbyGravity(frontContours,">y");
    frontTCP = tcpc::getHighestMiddle(frontContours,'x');
    sideTCP = tcpc::getHighestMiddle(sideeContours,'y');

    //Mark frames and show it
    MarkFrame(front_frame,frontTCP,this->settings.tcpc.calibration_Point.front);
    MarkFrame(side_frame,sideTCP,this->settings.tcpc.calibration_Point.side);

    //Emit Marked frames to UI
    emitMarkedFrames(front_frame, frontTCP, side_frame, sideTCP);
}

void Main_Algorithm::processing_3rd_position(cv::Point& frontTCP, cv::Point& sideTCP)
{
    cv::Mat front_frame, side_frame;
    //Get frames
    this->takeFrames(front_frame,side_frame);

    //Detect contours
    std::vector<cv::Point> frontContours = tcpc::getSignificantContours(front_frame, settings.HSV_filter.front, settings.cv.Canny_threshold,
                                                                            settings.cv.Dilate_thickness);
    std::vector<cv::Point> sideeContours = tcpc::getSignificantContours(side_frame, settings.HSV_filter.side, settings.cv.Canny_threshold,
                                                                                        settings.cv.Dilate_thickness);

    //Detect TCP in frames
    tcpc::filterPointsbyGravity(frontContours,">y");
    frontTCP = tcpc::getLowestMiddle(frontContours,'x');
    sideTCP = tcpc::getHighestMiddle(sideeContours,'y');

    //Mark frames and show it
    MarkFrame(front_frame,frontTCP,this->settings.tcpc.calibration_Point.front);
    MarkFrame(side_frame,sideTCP,this->settings.tcpc.calibration_Point.side);

    //Emit Marked frames to UI
    emitMarkedFrames(front_frame, frontTCP, side_frame, sideTCP);
}

ri::Point Main_Algorithm::estimateTCP(std::vector<ri::Point>detected_points)
{
/*********************************************INITS*********************************************/
    //std::vector<ri::Point> calib_positions;
    //ri::Point initial_gripper(20, 0, 250, 0, 0, 0);
    //calib_positions.push_back(ri::Point(155,416,166,46,146,-65));
    //calib_positions.push_back(ri::Point(427,476,171,27,-132,-49));
    //calib_positions.push_back(ri::Point(139,590,203,-17,139,-48));
    ri::Point init_gripper(settings.tcpc.default_gripper.x,settings.tcpc.default_gripper.y, settings.tcpc.default_gripper.z,0,0,0);
    ri::Point gripper;
    double best_global_fitness;
    std::stringstream ss;
    int MAX_ITER = 5000;

    std::default_random_engine gen;
    std::uniform_real_distribution<double> rand_dist(-1.0, 1.0);

    typedef Eigen::Matrix<double, 6, 1> vec6;
    double maxGripperDistance = 500.0;

    auto robot_point = [](vec6 params) {
        double radtodeg = 45 / std::atan(1);
        ri::Point pt(
            params[0], params[1], params[2],
            radtodeg * params[3], radtodeg * params[4], radtodeg * params[5]
        );
        return ri::Point(pt.matrix());
    };
    auto matrix = [](vec6 params) {
        return (
            Eigen::Affine3d(Eigen::Translation3d(params[0], params[1], params[2])) *
            Eigen::AngleAxisd(params[3], Eigen::Vector3d::UnitX()) *
            Eigen::AngleAxisd(params[4], Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(params[5], Eigen::Vector3d::UnitZ())
            ).matrix();
    };
    auto loss = [&](vec6 params) {
        return gripperOffsetLoss(detected_points, matrix(params));
    };
    auto random = [maxGripperDistance, &rand_dist, &gen, init_gripper]() {
        vec6 c;
        const double degtorad = std::atan(1) / 45;
        c << rand_dist(gen)*maxGripperDistance, rand_dist(gen)*maxGripperDistance, rand_dist(gen)*maxGripperDistance,
            degtorad * init_gripper.rx, degtorad * init_gripper.ry, degtorad * init_gripper.rz;
        return c;
    };
/**********************************************CALC*********************************************/
//INIT PSO
    struct Particle {
        vec6 x, best, velocity;
        double fitness, best_fitness;
    };
    vec6 best_global;
    best_global_fitness = std::numeric_limits<double>::max();

    std::vector<Particle> particles;

    // -- helper functions
    auto compute_best_global = [&]() {
        for (auto &p : particles) {
            if (p.fitness < best_global_fitness) {
                best_global_fitness = p.fitness;
                best_global = p.best;
            }
        }
    };

    int same_eps_term = 500;
    double c1 = 2.0, c2 = 2.0, w = 0.9;
    particles.resize(100);
    std::vector<double> fitness_rec;

    for (auto &p : particles) {
        p.x = random();
        p.fitness = loss(p.x);
        p.best = p.x;
        p.best_fitness = p.fitness;
        p.velocity << 0, 0, 0, 0, 0, 0;
    }
    compute_best_global();

    /************************/
    QMutex mux;
    mux.lock();
    if(this->stopFlag) return{};
    mux.unlock();
    /************************/

//MAIN LOOP PSO
    for (int i = 0; i < MAX_ITER; i++) {
        w -= (0.9 - 0.4) / MAX_ITER;
        for (auto &p : particles) {
            Eigen::Vector2d r; r << (rand_dist(gen) + 1.0) / 2.0, (rand_dist(gen) + 1.0) / 2.0;
            p.velocity = w * p.velocity + c1 * r[0] * (p.best - p.x) + c2 * r[1] * (best_global - p.x);
            p.x += p.velocity;
            p.fitness = loss(p.x);
            if (p.fitness < p.best_fitness) {
                p.best = p.x;
                p.best_fitness = p.fitness;
            }
        }
        compute_best_global();
        fitness_rec.emplace_back(best_global_fitness);

        /************************/
        QMutex mux;
        mux.lock();
        if(this->stopFlag) return{};
        mux.unlock();
        /************************/

        emit CalculationProgressChanged((double(i)/double(MAX_ITER))*100.0);
        if (int(fitness_rec.size()) >= same_eps_term) {
            bool same = true;
            for (size_t s = fitness_rec.size() - same_eps_term + 1; s < fitness_rec.size(); s++) {
                same = same && (fitness_rec[s - 1] == fitness_rec[s]);
            }
            if (same) {
                break;
            }
        }
    };

    gripper = robot_point(best_global);
/*********************************************STATS*********************************************/
    Eigen::Vector3d reference = computeReferencePoint(detected_points, gripper);
    emit CalculationProgressChanged(100.0);

    ss<<"TCP:\n";
    ss<<gripper;
    ss<<"\nError: "<< best_global_fitness <<"mm";
    ss<<"\nPrecision: "<<calibQuality(detected_points) * 100.0 << "%";
    ss<<"\nStationary point: "<< reference.x() << "mm, y: " << reference.y() << "mm, z: " << reference.z() <<"mm";
    this->resLog = ss.str().c_str();
    std::cout << this->resLog.toStdString() << std::endl;
    return gripper;
}

QString Main_Algorithm::getLog()
{
    return this->resLog;
}

Eigen::Matrix4d Main_Algorithm::pointToMatrix(ri::Point pt)
{
    double degtorad = std::atan(1) / 45.0;
    Eigen::Affine3d r(
        Eigen::AngleAxisd(pt.rx * degtorad, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(pt.ry * degtorad, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(pt.rz * degtorad, Eigen::Vector3d::UnitZ())
    );
    Eigen::Affine3d t(Eigen::Translation3d(pt.x, pt.y, pt.z));
    return (t * r).matrix();
}

double Main_Algorithm::calibQuality(std::vector<ri::Point> calibPositions)
{
    double max_quality = 0.0;
    for (int c = 0; c < int(calibPositions.size()); c++) {
        for (int b = 0; b < int(calibPositions.size()); b++) {
            for (int a = 0; a < int(calibPositions.size()); a++) {
                if (a == b || b == c || c == a) {
                    continue;
                }
                Eigen::Matrix4d robot_mat_a = pointToMatrix(calibPositions[a]);
                Eigen::Matrix4d robot_mat_b = pointToMatrix(calibPositions[b]);
                Eigen::Matrix4d robot_mat_c = pointToMatrix(calibPositions[c]);
                double quality = Eigen::Vector3d{
                    ((robot_mat_a * Eigen::Vector4d::UnitX()).head<3>()).dot(
                        (robot_mat_b * Eigen::Vector4d::UnitX()).head<3>().cross(
                        (robot_mat_c * Eigen::Vector4d::UnitX()).head<3>())),
                    ((robot_mat_a * Eigen::Vector4d::UnitY()).head<3>()).dot(
                        (robot_mat_b * Eigen::Vector4d::UnitY()).head<3>().cross(
                        (robot_mat_c * Eigen::Vector4d::UnitY()).head<3>())),
                    ((robot_mat_a * Eigen::Vector4d::UnitZ()).head<3>()).dot(
                        (robot_mat_b * Eigen::Vector4d::UnitZ()).head<3>().cross(
                        (robot_mat_c * Eigen::Vector4d::UnitZ()).head<3>())),
                }.cwiseAbs().maxCoeff();
                max_quality = std::max(max_quality, quality);
            }
        }
    }
    return max_quality;
}

Eigen::Vector3d Main_Algorithm::computeReferencePoint(std::vector<ri::Point> calibPositions, ri::Point gripper)
{
    Eigen::Vector4d mean = { 0,0,0,0 };
    for (int i = 0; i < int(calibPositions.size()); i++) {
        mean += pointToMatrix(calibPositions[i]) * pointToMatrix(gripper) * Eigen::Vector4d{ 0,0,0,1 };
    }
    mean /= mean.w();
    return mean.head<3>();
}

double Main_Algorithm::gripperOffsetLoss(std::vector<ri::Point> calibPositions, Eigen::Matrix4d gripperMatrix)
{
    std::vector<Eigen::Vector4d> lossPoints;

    Eigen::Vector3d mean = { 0,0,0 };
    Eigen::Vector3d sd = { 0,0,0 };

    for (int i = 0; i < int(calibPositions.size()); i++) {
        Eigen::Vector4d p = pointToMatrix(calibPositions[i]) * gripperMatrix * Eigen::Vector4d{ 0,0,0,1 };
        lossPoints.emplace_back(p);
    }
    for (int i = 0; i < int(lossPoints.size()); i++) {
        mean += lossPoints[i].head<3>();
    }
    mean /= static_cast<double>(lossPoints.size());
    for (int i = 0; i < int(lossPoints.size()); i++) {
        auto delta = lossPoints[i].head<3>() - mean;
        sd += delta.cwiseProduct(delta);
    }
    sd = (sd / (static_cast<double>(lossPoints.size()) - 1.0)).cwiseSqrt();

    return sqrt(sd.squaredNorm());
}
