void MainWin::on_actionRobot_Camera_Calibration_triggered()
{
    if(this->robot==nullptr){
        QMessageBox text;
        text.setWindowTitle("ERROR");
        text.setText("Robot NOT setted");
        text.exec();
        return;
    }else if(this->front_camera==nullptr || this->side_camera==nullptr){
        QMessageBox text;
        text.setWindowTitle("ERROR");
        text.setText("Cameras NOT setted");
        text.exec();
    }else{
        if(this->parameters.Robot.default_position != this->robot->getPosition()) robot->moveTO(this->parameters.Robot.default_position);
    }

    //Inits
    const int N = 10;
    cv::Mat_<cv::Point2f> pointsF(1,N), pointsS(1,N);
    std::vector<cv::Point3f> robot3D, camera3D;
    robot_interface::sRobot_point position;

    for(int i=0;i<N;i++){
        on_shotF_button_clicked();
        on_shotS_button_clicked();

        robot3D.push_back({float(this->robot->getPosition().x), float(this->robot->getPosition().y), float(this->robot->getPosition().z)});

        std::vector<cv::Point> contoursF = tcpc::getSignificantContours(front_frame.image, this->parameters.HSV.front, this->parameters.Front_view.Canny_threshold,
                                                                                this->parameters.Front_view.Dilate_thickness);
        std::vector<cv::Point> contoursS = tcpc::getSignificantContours(side_frame.image, this->parameters.HSV.right, this->parameters.Side_view.Canny_threshold,
                                                                                this->parameters.Side_view.Dilate_thickness);
        pointsF(i) = tcpc::getHighestPoint(contoursF,'y');
        pointsS(i) = tcpc::getHighestMiddle(contoursS,'y');

        if(i==N-1){
            robot->moveTO(this->parameters.Robot.default_position);
        }else{
            position = this->parameters.Robot.default_position;
            position.x += (rand() % 100) -50;
            position.y += (rand() % 100) -50;
            position.z += (rand() % 100) -50;
            robot->moveTO(position);
        }
    }

    cv::undistortPoints(pointsF, pointsF, this->camsCalib.Front_cam.K_matrix, this->camsCalib.Front_cam.Dist_coeffs);
    cv::undistortPoints(pointsS, pointsS, this->camsCalib.Side_cam.K_matrix, this->camsCalib.Side_cam.Dist_coeffs);

    for(int i=0;i<N;i++){
        cv::Point3f point;
        tcpc::triangulatePoint(pointsF(i), pointsS(i), point, this->camsCalib.Q_matrix);
        camera3D.push_back(point);
    }

    cv::Mat calib_point = cv::Mat_<float>(3,1);
    cv::Mat_<cv::Point2f> calibPointF(1,1), calibPointS(1,1);
    calibPointF(0) = cv::Point2f(this->calibration_Point.front.x,this->calibration_Point.front.y);
    calibPointS(0) = cv::Point2f(this->calibration_Point.side.x,this->calibration_Point.side.y);
    cv::undistortPoints(calibPointF, calibPointF, this->camsCalib.Front_cam.K_matrix, this->camsCalib.Front_cam.Dist_coeffs);
    cv::undistortPoints(calibPointS, calibPointS, this->camsCalib.Front_cam.K_matrix, this->camsCalib.Front_cam.Dist_coeffs);
    tcpc::triangulatePoint(calibPointF(0), calibPointS(0), calib_point, this->camsCalib.Q_matrix);

    cv::Mat Rot, tran;
    tcpc::rigidTransform(camera3D, robot3D, Rot, tran);

    cv::Mat estimated_Position = cv::Mat_<float>(3,1);
    estimated_Position = (Rot*calib_point)+tran;

    robot_interface::sRobot_point estimatedRobot_Position = this->parameters.Robot.default_position;
    estimatedRobot_Position.fromMatrix_OnlyCoordinates(estimated_Position);
    robot->moveTO(estimatedRobot_Position);

    cv::drawMarker(front_frame.image, this->calibration_Point.front,  cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 40, 10);
    cv::drawMarker(side_frame.image, this->calibration_Point.side,  cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 40, 10);
    on_ShowFrames(this->front_frame,this->side_frame);
} 
