text:
/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
--------------------------------------------------------------*/

#include "sdd_vio/vo_stereo.h"

#include <iostream>
#include "sdd_vio/utils/math_utils.h"
#include <algorithm>
#include <math.h>
#include <ros/ros.h>
#include <cmath>
#include <assert.h>
#include <boost/thread.hpp>
#include "sdd_vio/utils/ros_params_helper.h"
#include "sdd_vio/utils/timer.hpp"
#include <ctime>
#include <opencv2/core/eigen.hpp>

namespace sdd_vio {

/* extract high gradient pixels */
// Input: left image 'img0'
// Output: high gradient pixels mask 'G_binary', gradient mat 'Gx','Gy'
void VoStereo::extractFeats(const cv::Mat& img0, cv::Mat& G_binary, cv::Mat& Gx, cv::Mat& Gy, int ilay)
{
  ROS_DEBUG_STREAM("extract-Feats: ----------");

  /* get gradient - from the tracking base layer and each layer up */
  ROS_DEBUG_STREAM("extract-Feats: get gradient");
  cv::Mat Gx2, Gy2, G2, G;
  cv::Sobel(img0, Gx, CV_32F, 1, 0, 3);
  cv::Sobel(img0, Gy, CV_32F, 0, 1, 3);
  cv::multiply(Gx, Gx, Gx2);
  cv::multiply(Gy, Gy, Gy2);
  cv::add(Gx2, Gy2, G2);
  cv::sqrt(G2, G);

  G = sdd_vio::adjustVis(G);  // adjust to 0-255
  G.convertTo(G, CV_8U);
  // sdd_vio::showMatMinMax(G);

  /*  threshold gradient - on the base layer */
  ROS_DEBUG_STREAM("extract-Feats: threshold gradient");
  cv::adaptiveThreshold(G, G_binary, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, adapt_size_, adapt_thresh_);
  // double thresh_otsu;
  // thresh_otsu = cv::threshold(G, G_binary, 0, 255, cv::THRESH_OTSU);
  // std::cout << "thresh otsu: " << thresh_otsu << std::endl;
  // cv::threshold(G2, G2_binary, 254, 255, cv::THRESH_BINARY);

  // /* prune function that changes G_binary to contain only selected points */
  ROS_DEBUG_STREAM("extract-Feats: prune function");
  if (use_gd_)
    gd_[ilay]->prune(G,G_binary);

  /* set margin - for selection not too close to the boundary */
  // std::cout<<"G_Binar at 0,0: "<<G_binary.at(0,0)<<"\n";
  int nrows = G_binary.rows;
  int ncols = G_binary.cols;
  G_binary.rowRange(0, feat_margin_).setTo(cv::Scalar(0));
  G_binary.rowRange(nrows-feat_margin_, nrows).setTo(cv::Scalar(0));
  G_binary.colRange(0, feat_margin_).setTo(cv::Scalar(0));
  G_binary.colRange(ncols-feat_margin_, ncols).setTo(cv::Scalar(0));

}

/*
Fill out vectors given mask
    Input:
        img0: keyframe image
        G_binary: binary mask of selected high gradient points
        disp: disparity image
        Gx: gradient of feature points in x direction
        Gy: gradient of feature points in y direction
        ilay: specifying the layer number

    Output:
        feat_pixels: the pixel coordinates of feature points
        disp_vec: disparity of feature points
        intensities: pixel intensity of feature points
        Gx_vec: Gx of feature points
        Gy_vec: Gy of feature points
        num_pts: total number of feature points
*/
    void VoStereo::fillVectors(const cv::Mat& img0, const cv::Mat& G_binary, const cv::Mat& disp,
            const cv::Mat& Gx, const cv::Mat& Gy, std::vector<float>& disp_vec, vector_aligned<Eigen::Vector2i>& feat_pixels,
            std::vector<float>& intensities, std::vector<float>& Gx_vec, std::vector<float>& Gy_vec, int& num_pts, int ilay)
    {

        disp_vec.clear();
        feat_pixels.resize(0);
        intensities.resize(0);
        Gx_vec.resize(0);
        Gy_vec.resize(0);

        std::vector<cv::Point> high_gradient_pixels;
        cv::findNonZero(G_binary, high_gradient_pixels);
        int npts = high_gradient_pixels.size();

        /* get disparity vector and filter out unwanted points */
        for (int i=0; i<npts; ++i)
        {
            int x = high_gradient_pixels[i].x;
            int y = high_gradient_pixels[i].y;
            if (!inBound(x,y,0, ilay))
                std::cout<< "not inbound!" << std::endl;
            float d = disp.at<int16_t>(y,x)/16.0;
            float gx = Gx.at<float>(y,x);
            float gy = Gy.at<float>(y,x);
            float intensity = float(img0.at<uchar>(y,x));

            /* select valid disparity range */
            if (d>disp_range_pyr_[ilay] && inBound(x,y,1,ilay)) {
                disp_vec.push_back(d);
                Eigen::Vector2i pixel(x,y);
                feat_pixels.push_back(pixel);
                Gx_vec.push_back(gx);
                Gy_vec.push_back(gy);
                intensities.push_back(intensity);
            }
        }
        // showVecMinMax(Gx_);
        // std::cout << "valid number of feature points: " << disp_vec.size() << std::endl;
        // sdd_vio::showVecMinMax(disp_vec);
        num_pts = disp_vec.size();
    }

/*
    Input:
        Gx: gradient vector of feature points in x direction
        Gy: gradient vector of feature points in y direction
        fx: calibration param
        fy: calibration param
        feat_3D: feature point coordinates in 3D in keyframe
        npts: umber of feature points

    Output:
        J: Jacobian matrix of the feature points
*/
    void VoStereo::getJacobian(const std::vector<float>& Gx, const std::vector<float>& Gy,
            float fx, float fy, const vector_aligned<Eigen::Vector3f>& feat_3D, Eigen::MatrixXf& J, int npts)
    {
        J.resize(npts,6);
        Eigen::Matrix<float,1,6> J_i; // Jacobian for one point
        Eigen::Matrix<float,1,2> J_grad; // gradient jacobian
        Eigen::Matrix<float,2,3> J_proj; // projection jacobian
        Eigen::Matrix<float,3,6> J_SE3; // exponential jacobian
        for (int i=0; i<npts; ++i)
        {
            /* calculate for each point the 1x6 Jacobian */
            float x = feat_3D[i](0);
            float y = feat_3D[i](1);
            float z = feat_3D[i](2);

            J_grad(0,0) = Gx[i];
            J_grad(0,1) = Gy[i];

            J_proj(0,0) = fx/z;
            J_proj(1,0) = 0;
            J_proj(0,1) = 0;
            J_proj(1,1) = fy/z;
            J_proj(0,2) = -fx*x/(z*z);
            J_proj(1,2) = -fy*y/(z*z);

            Eigen::Matrix3f npHat;
            npHat << 0,z,-y,-z,0,x,y,-x,0;
            J_SE3 << Eigen::Matrix3f::Identity(3,3), npHat;

            J_i = J_grad * J_proj * J_SE3;

            // if (i==0) {
            //  std::cout<<"x,y,z:\n"<<x<<'\n'<<y<<'\n'<<z<<std::endl;
            //  std::cout<<"J_grad:\n"<<J_grad<<std::endl;
            //  std::cout<<"J_proj:\n"<<J_proj<<std::endl;
            //  std::cout<<"J_SE3:\n"<<J_SE3<<std::endl;
            //  std::cout<<"J_i:\n"<<J_i<<std::endl;
            // }

            /* assign J_i to J */
            J.block<1,6>(i,0) = J_i;
        }

    }  // void VoStereo::getJacobian

/*
    Input:
        img_curr: current frame image
        T_curr: current optimized transformation T between current frame and keyframe
        feat_3D: feature points in 3D in keyframe coordinates
        npts: number of feature points in total in the keyframe
        ilay: specifying the layer from image pyramid to get error from
        intensities: intensities of feature points from keyframe
        J: Jacobian calculated from keyframe on the feature points

    Output:
        feat_3D_curr: feature points in 3D in current frame coordinates
        feat_pixels_curr: feature points in current frame pixel coordinates
        npts_in: the number of points remaining in view
        mask: vector of size feat_3D, 0 if not in view, 1 if in view
        error: vector of intensity errors, size as feat_3D, only update the values when mask=1
        J_masked: of size npts_in x 6, with only entries of mask=1
        error_masked: of size npts_in x 6, with only entries of mask=1
        W: diagonal weight matrix for robust estimation Huber weights
*/
    void VoStereo::getError(const cv::Mat& img_curr, const Eigen::MatrixXf& J, const Eigen::Isometry3f& T_curr,
            const vector_aligned<Eigen::Vector3f>& feat_3D, const std::vector<float>& intensities,
            vector_aligned<Eigen::Vector3f>& feat_3D_curr, vector_aligned<Eigen::Vector2f>& feat_pixels_curr, Eigen::VectorXf& error,
            Eigen::VectorXf& mask, const int npts, const int ilay, int& npts_in,
            Eigen::MatrixXf& J_masked, Eigen::VectorXf& error_masked, Eigen::VectorXf& W)
    {

        /* Timer Template
        ros::Time t1 = ros::Time::now();
        ros::Time t2 = ros::Time::now();
        ros::Duration duration = t2 - t1;
        std::cout << "Time for sorting: " << duration.toSec() << std::endl;
        */

        ROS_DEBUG_STREAM("get-Error: --------- ");

        /* transformation of feat_3D into current camera frame and obtain feat_3D_curr */
        transform3D(feat_3D, feat_3D_curr, T_curr, npts);

        /* for debug - will print out info if nan values found */
        bool feat3dhasnan = false;
        bool feat3dcurrhasnan = false;
        for (int i=0; i<npts; ++i){
            if (std::isnan(feat_3D_curr[i](2)))
                feat3dcurrhasnan = true;
            if (std::isnan(feat_3D[i](2)))
                feat3dhasnan = true;
        }
        if (feat3dhasnan == true)
            std::cout << "feat_3D has nan"<< std::endl;
        if (feat3dcurrhasnan == true){
            std::cout << "feat_3D_curr has nan"<<std::endl;
            // std::cout<<"T_curr: "<<'\n'<<T_curr<<std::endl;
        }
        /* for debug */

        /* reproject into current camera image */
        cam_->get2DPixels(feat_3D_curr, feat_pixels_curr, npts, ilay);

        /* update error vector and mask */
        npts_in = 0;
        for (int i=0; i<npts; ++i) {
            if (inBound(feat_pixels_curr[i](0),feat_pixels_curr[i](1),0, ilay)) {
                error[i] = intensities[i]-interpolate(feat_pixels_curr[i](0),feat_pixels_curr[i](1),img_curr);
                npts_in = npts_in+1;
                mask[i] = 1;
            }
            else {
                mask[i] = 0;  // mark the point as out of bound and not to consider
            }
        }

        /* fill in the masked Jacobian */
        J_masked.resize(npts_in,6);
        error_masked.resize(npts_in);
        int ind = 0;
        for (int i=0; i<npts; ++i) {
            if (mask[i]!=0) {
                J_masked.row(ind) = J.row(i);
                error_masked[ind] = error[i];
                ind++;
            }
        }

        Eigen::VectorXf r = error_masked;
        // Eigen::VectorXf r_abs = error_masked.cwiseAbs();

        /* sort to find median - impossible for time */
        // std::sort(r.data(),r.data()+r.size());
        // std::sort(r_abs.data(),r_abs.data()+r_abs.size());
        // int mid = (int)(npts_in/2);
        // float r_med = r[mid-1];
        // float r_abs_med = r_abs[mid-1];
        // Eigen::VectorXf r_hat = error_masked.array() - r_med;  // normalize by removing the median (original order)
        // float sigma = 1.485 * r_abs_med;  // median absolute deviation

        float r_mean = r.mean();
        Eigen::VectorXf r_hat = error_masked.array() - r_mean;  // normalize by removing the mean (original order)
        float sigma = sqrt(((r_hat.array()).square()).sum()/npts_in);  // standard deviation
        // std::cout << "sigma: " << sigma << std::endl;

        W.resize(npts_in);
        for (int i=0; i<npts_in; ++i) {
            if (fabs(r_hat[i]) < 1.345*sigma)
                W[i] = 1;
            else
                W[i] = (1.345*sigma)/fabs(r_hat[i]);
        }

        /* for debug */
        if (J_masked.maxCoeff() > J.maxCoeff()){
            std::cout << "ind: " << ind << std::endl;
            std::cout << "npts_in: "<<npts_in<<std::endl;
            std::cout << "mask_sum: "<<mask.sum()<<std::endl;
        }
        /* for debug */

    }

/*
    Input:
        img_curr: current image
        J: Jacobian calculated from last keyframe of size npts x 6
        feat_pixels:
        feat_3D: feature points in 3D in keyframe coordinates
        intensities: pixel intensities of feat_3D points
        npts: number of feature points in keyframe
        max_iter: max number of iterations specified for the optimizer
        ilay: specifying the layer in image pyramid

    Updates:
        T_curr: current optimized transformation from keyframe to current frame
        error: intensity errors of size npts x 1
        variance:
        mask: 0 if not in current view, 1 if in view
*/
    void VoStereo::optimize(const cv::Mat& img_curr, const Eigen::MatrixXf& J, Eigen::Isometry3f& T_curr, Eigen::VectorXf& error,
            Eigen::VectorXf& variance, vector_aligned<Eigen::Vector2i>& feat_pixels, vector_aligned<Eigen::Vector3f>& feat_3D,
            std::vector<float>& intensities, Eigen::VectorXf& mask, const int npts, int max_iter, int ilay)
    {
        ROS_DEBUG_STREAM("op-timize: ----------- ");

        int npts_in;  // number of points remained in view
        perc_ = 1.;  // percentage of points remained in view
        float err, err_mean = 0, err_init, err_last, err_diff = 0;

        vector_aligned<Eigen::Vector3f> feat_3D_curr;  // 3D points in current camera's frame
        vector_aligned<Eigen::Vector2f> feat_pixels_curr;  // 2D pixel coordinates in current image

        /* masked Jacobian matrix and error vector - remove points outside projection */
        Eigen::MatrixXf J_masked;
        Eigen::VectorXf error_masked;
        Eigen::VectorXf W;  // diagonal weight matrix for Huber estimation
        Eigen::MatrixXf J_masked_weighted;

        /* Levenberg Marquardt */
        float lambda = lambda_;  // initial lambda is equal to the initial one set
        float up = up_factor_;
        float down = down_factor_;
        bool if_break = false;

        for (int iter=0; iter<max_iter; ++iter)
        {
            // vis_curr = vis_curr.clone();
            // vis_curr = vis_curr;

            /* calculate error vector and update mask given initial transformation T_curr */
            if (iter == 0) {

                /*
                Calculate errors and Jacobians for optimization
                    Inputs:
                        img_curr
                        J
                        T_curr
                        feat_3D
                        intensities
                        npts
                        ilay
                    Outputs:
                        npts_in: for statistics
                        J_masked: for Hessian calculation
                        error_masked: for Hessian calculation,
                        feat_pixels_curr: for visualization
                    Recalculate in every iteration:
                        error
                        mask
                        feat_3D_curr
                */
                getError(img_curr, J, T_curr, feat_3D, intensities, feat_3D_curr, feat_pixels_curr, error, mask,
                        npts, ilay, npts_in, J_masked, error_masked, W);
                ROS_DEBUG_STREAM("op-timize: initial error obtained ");

                /* Monitoring
                    getting statistics for setting criteria for end of iterations:
                    percentage of points in view and current average error value */
                perc_ = 100*npts_in/npts;
                err = error_masked.transpose()*error_masked; // sum of square errors
                err_mean = sqrt(err/npts_in);  // NOTE: not technically mean, but mean on squares then do sqrt
                err_init = err_mean;

                // draw blue circles on vis_curr of all the points
                if (use_opt_vis_){
                    for (int i=0; i<npts; ++i) {
                        cv::Point p(feat_pixels_curr[i](0),feat_pixels_curr[i](1));
                        cv::circle(vis_curr, p,1,cv::Scalar(255,0,0),0.5);
                    }
                }
            }

            /* convert weight vector W to diagonal matrix */
            // J_masked_weighted.resize(npts_in,6);
            // for (int i=0; i<npts_in; ++i) {
            //     J_masked_weighted.row(i) = J_masked.row(i) * W[i];
            // }

            /* get H = J^t * W * J */
            Eigen::MatrixXf H = J_masked.transpose() * W.asDiagonal() * J_masked;
            // Eigen::MatrixXf H = J_masked_weighted.transpose() * J_masked;

            /* get b = J^t * w * e */
            Eigen::VectorXf b = J_masked.transpose() * (W.asDiagonal() * error_masked);
            // Eigen::VectorXf b = J_masked_weighted.transpose() * error_masked;

            ROS_DEBUG_STREAM("op-timize: Hessian obtained. ");

            /* Monitoring */
            err_last  = err_mean;

            /* LMA */
            float mult = 1 + lambda;
            bool ill = true;  // if ill conditioned (error increase)
            Eigen::Isometry3f T_temp;
            int lmit = 0;  // number of iterations while ill=true

            while ((ill==true) && (iter<max_iter) && (lmit<5))
            {
                /* Get H based on LMA current damping parameter, multiply diagonal by mult */
                for (int par=0; par<6; par++)
                    H(par,par) = H(par,par)*mult;

                /* get delta increment */
                Eigen::VectorXf delta;
                delta = -H.ldlt().solve(b);

                /* for debug - delta is valid */
                if (std::isnan(delta(0)+delta(1)+delta(2)+delta(3)+delta(4)+delta(5))){
                    std::cout<<"delta:"<<'\n'<<delta<<std::endl;
                    std::cout<<"H"<<'\n'<<H<<std::endl;
                    std::cout<<"b"<<'\n'<<b<<std::endl;
                    showEigenInfo(J_masked);
                    showEigenInfo(error_masked);
                    showEigenInfo(J);
                    showEigenInfo(error);
                }
                /* for debug */

                /* update current transformation */
                T_temp = T_curr * (exp_SE3(delta).inverse());

                /* calculate error vector and update mask given transformation */
                getError(img_curr, J, T_temp, feat_3D, intensities, feat_3D_curr, feat_pixels_curr, error, mask,
                        npts, ilay, npts_in, J_masked, error_masked, W);

                perc_ = 100*npts_in/npts;
                err = error_masked.transpose()*error_masked;
                err_mean = sqrt(err/npts_in);

                /* get error increment */
                err_diff = err_mean - err_last;
                ill = (err_diff > 0);  // if error larger than before, set ill = true, try increase lambda.

                /* terminal output */
                if (verbose_) {
                    std::cout << "iteration = "<<iter<<", lambda = "<<lambda<<", err = "<<
                        err_mean<<", derr = "<<err_diff<<std::endl;
                }

                /* if ill=true, higher lambda by up factor, count as one iteration */
                if (ill) {
                    mult = (1+lambda*up)/(1+lambda);
                    lambda *= up;
                    iter++;
                }

                lmit++;
            }

            /* update T with the one that makes error smaller or the last try */
            /* error and jacobians are already updated in the loop */
            T_curr = T_temp;

            /* if error doesn't get higher for quite some time, this term will bring lambda down eventually */
            if (lambda > 1e-8)  // make sure doesn't overflow
                lambda *= (1/down);

            /* if LM iterations didn't decrease the error, stop */
            if (ill) {
                // ROS_WARN_STREAM("bad convergence!");
                if_break = true;
            }

            /* if error is decreased below target or reach max iterations */
            if (-err_diff < target_derr_ || iter == max_iter-1)
                if_break = true;

            /* end of optimization */
            if (if_break) {

                ROS_INFO_STREAM("iteration "<<iter<<"--------");
                ROS_INFO_STREAM("err/initial err: "<<err_mean<<"/"<<err_init);

                // optimized position, draw green circles
                if (use_opt_vis_) {
                    for (int i=0; i<npts; ++i) {
                        if (inBound(feat_pixels_curr[i](0),feat_pixels_curr[i](1),0,ilay)) {
                            cv::Point p(feat_pixels_curr[i](0),feat_pixels_curr[i](1));
                            cv::circle(vis_curr, p,1,cv::Scalar(0,255,0),0.5);
                        }
                    }
                    cv::imshow("current features", vis_curr);
                    cv::waitKey(1);
                }
                break;
            }

        } // for (int iter=0;

    }  //void VoStereo::op-timize

/*
    Optimization for state x given current frame image using Ceres-Solver

    x: 6x1 state vector. first three elements in so(3) of R_curr_, last three elements same as t_curr_
    Ceres is using AutoDiff to find the Jacobians of photometric errors with respect to state x

    Input:
        img_curr: current left camera image
        ilay: the layer that it is on (e.g. there're two layers in total, then 0 is the larger and 1 is the smaller)
    Updates:
        R_curr_: 3x3 rotation matrix from keyframe to current frame
        t_curr_: 3x1 translation vector from keyframe to current frame
        T_curr_: 4x4 SE3 transformation matrix from keyframe to current frame
*/
//    void VoStereo::optimize_ceres(const cv::Mat& img_curr, int ilay)
//    {
//        // The variable to solve for with its initial value.
//        Eigen::Matrix<double,3,1> phi_init = log_SO3<double>(R_curr_.cast<double>());
//        double x[6] = {phi_init(0), phi_init(1), phi_init(2), t_curr_(0), t_curr_(1), t_curr_(2)};

//        // Build the problem.
//        ceres::Problem problem;

//        // Set up the cost function (also known as residual). This uses
//        // auto-differentiation to obtain the derivative (jacobian).
//        ceres::CostFunction* cost_function =
//            new ceres::AutoDiffCostFunction<pixelIntensityError, ceres::DYNAMIC, 6>(new pixelIntensityError(this, &img_curr, ilay), num_feat_pts_[ilay]);
//        problem.AddResidualBlock(cost_function, NULL, x);

//        // Run the solver!
//        ceres::Solver::Options options;
//        options.minimizer_progress_to_stdout = false;
//        options.max_num_iterations = 30;
//        ceres::Solver::Summary summary;
//        ceres::Solve(options, &problem, &summary);

//        std::cout << summary.BriefReport() << "\n";
////        std::cout << summary.FullReport() << "\n";

//        /* construct R and t from x */
//        Eigen::Matrix<float,3,1> phi, p;  // state variables os so3 and translation
//        phi(0) = x[0]; phi(1) = x[1]; phi(2) = x[2];
//        p(0) = x[3]; p(1) = x[4]; p(2) = x[5];
//        Eigen::Matrix<float,3,3> R_curr = exp_SO3<float>(phi);

//        /* assign R_curr_ and t_curr_, T_curr_ with optimized values */
//        R_curr_ = R_curr;
//        t_curr_ = p;
//        T_curr_ = TfromRt(R_curr, p);
//        rot_curr_ = phi.norm();

//    }

}


text:
// Copyright (C) 2015 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/make.hpp>

//  template <template <class ...> class M, class X>
//  auto make(X&& x);
//  template <class M, class X>
//  auto make(X&& x);
//  template <class M, class ...Args>
//  auto make(Args&& ...args);

#include <experimental/make.hpp>
#include <experimental/meta.hpp>
#include <experimental/tuple.hpp>
#include <boost/detail/lightweight_test.hpp>

int main()
{
  namespace stde = std::experimental;
  {
    int v=0;
    std::tuple<int, int> x = stde::make<std::tuple>(v, v);
    BOOST_TEST(std::get<0>(x) == 0);
    BOOST_TEST(std::get<1>(x) == 0);
  }
  {
    int v=0;
    std::tuple<long, long> x = stde::make<std::tuple<long, long> >(v, v);
    BOOST_TEST(std::get<0>(x) == 0);
    BOOST_TEST(std::get<1>(x) == 0);
  }
  {
    int v=0;
    std::tuple<int, int> x = stde::make<std::tuple<stde::_t> >(v, v);
    BOOST_TEST(std::get<0>(x) == 0);
    BOOST_TEST(std::get<1>(x) == 0);
  }
#if __cplusplus >= 201402L
  {
    constexpr int v=0;
    constexpr std::tuple<int, int> x = stde::make<std::tuple>(v, v);
    BOOST_TEST(std::get<0>(x) == 0);
    BOOST_TEST(std::get<1>(x) == 0);
  }
#endif
  return ::boost::report_errors();
}

ext: cpp

max_stars_repo_name: jwakely/std-make

max_issues_repo_name: jwakely/std-make

max_forks_repo_name: jwakely/std-make


text:
/*
	This file is part of solidity.

	solidity is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	solidity 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 solidity.  If not, see <http://www.gnu.org/licenses/>.
*/
/**
 * Unit tests for the mini moustache class.
 */

#include <libsolutil/Whiskers.h>

#include <test/Common.h>

#include <boost/test/unit_test.hpp>

using namespace std;

namespace solidity::util::test
{

BOOST_AUTO_TEST_SUITE(WhiskersTest)

BOOST_AUTO_TEST_CASE(no_templates)
{
	string templ = "this text does not contain templates";
	BOOST_CHECK_EQUAL(Whiskers(templ).render(), templ);
}

BOOST_AUTO_TEST_CASE(basic_replacement)
{
	string templ = "a  x <c> -> <d>.";
	string result = Whiskers(templ)
		("b", "BE")
		("c", "CE")
		("d", "DE")
		.render();
	BOOST_CHECK_EQUAL(result, "a BE x CE -> DE.");
}

BOOST_AUTO_TEST_CASE(tag_unavailable)
{
	string templ = "";
	Whiskers m(templ);
	BOOST_CHECK_THROW(m.render(), WhiskersError);
}

BOOST_AUTO_TEST_CASE(list_unavailable)
{
	string templ = "<#b>";
	Whiskers m(templ);
	BOOST_CHECK_THROW(m.render(), WhiskersError);
}

BOOST_AUTO_TEST_CASE(name_type_collision)
{
	string templ = "<#b>";
	Whiskers m(templ);
	m("b", "x");
	BOOST_CHECK_THROW(m("b", vector<map<string, string>>{}), WhiskersError);
}

BOOST_AUTO_TEST_CASE(conditional)
{
	string templ = "<?b>X";
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", true).render(), "X");
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", false).render(), "");
}

BOOST_AUTO_TEST_CASE(conditional_with_else)
{
	string templ = "<?b>X<!b>Y";
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", true).render(), "X");
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", false).render(), "Y");
}

BOOST_AUTO_TEST_CASE(conditional_plus_params)
{
	string templ = " - <?b>_<r><!b>^<t> - ";
	Whiskers m1(templ);
	m1("b", true);
	m1("r", "R");
	m1("t", "T");
	BOOST_CHECK_EQUAL(m1.render(), " - _R - ");

	Whiskers m2(templ);
	m2("b", false);
	m2("r", "R");
	m2("t", "T");
	BOOST_CHECK_EQUAL(m2.render(), " - ^T - ");
}

BOOST_AUTO_TEST_CASE(conditional_plus_list)
{
	string templ = " - <?b>_<#l><x></l><!b><#l><y></l> - ";
	Whiskers m(templ);
	m("b", false);
	vector<map<string, string>> list(2);
	list[0]["x"] = "1";
	list[0]["y"] = "a";
	list[1]["x"] = "2";
	list[1]["y"] = "b";
	m("l", list);
	BOOST_CHECK_EQUAL(m.render(), " - ab - ");
}

BOOST_AUTO_TEST_CASE(string_as_conditional)
{
	string templ = "<?+b>+<!+b>-</+b>";
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", "abc").render(), "+abc");
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", "").render(), "-");
}

BOOST_AUTO_TEST_CASE(string_as_conditional_wrong)
{
	string templ = "<?+b>+";
	BOOST_CHECK_EQUAL(Whiskers(templ)("b", "abc").render(), "<?+b>+abc");
}

BOOST_AUTO_TEST_CASE(complicated_replacement)
{
	string templ = "a  x <complicated> \n <nested>>.";
	string result = Whiskers(templ)
		("b", "BE")
		("complicated", "CO<M>PL")
		("nested", "NEST")
		.render();
	BOOST_CHECK_EQUAL(result, "a BE x CO<M>PL \n NEST>.");
}

BOOST_AUTO_TEST_CASE(non_existing_list)
{
	string templ = "a <#b>";
	Whiskers m(templ);
	BOOST_CHECK_THROW(m.render(), WhiskersError);
}

BOOST_AUTO_TEST_CASE(empty_list)
{
	string templ = "a <#b>x";
	string result = Whiskers(templ)("b", vector<Whiskers::StringMap>{}).render();
	BOOST_CHECK_EQUAL(result, "a x");
}

BOOST_AUTO_TEST_CASE(list)
{
	string templ = "a<#b>( <g> - <h> )x";
	vector<map<string, string>> list(2);
	list[0]["g"] = "GE";
	list[0]["h"] = "H";
	list[1]["g"] = "2GE";
	list[1]["h"] = "2H";
	string result = Whiskers(templ)("b", list).render();
	BOOST_CHECK_EQUAL(result, "a( GE - H )( 2GE - 2H )x");
}

BOOST_AUTO_TEST_CASE(recursive_list)
{
	// Check that templates resulting from lists are not expanded again
	string templ = "a<#b> 1<g>3 <x>";
	vector<map<string, string>> list(1);
	list[0]["g"] = "<x>";
	string result = Whiskers(templ)("x", "X")("b", list).render();
	BOOST_CHECK_EQUAL(result, "a 1<x>3 X");
}

BOOST_AUTO_TEST_CASE(list_can_access_upper)
{
	string templ = "<#b>()";
	vector<map<string, string>> list(2);
	Whiskers m(templ);
	string result = m("a", "A")("b", list).render();
	BOOST_CHECK_EQUAL(result, "(A)(A)");
}

BOOST_AUTO_TEST_CASE(parameter_collision)
{
	string templ = "a <#b>";
	vector<map<string, string>> list(1);
	list[0]["a"] = "x";
	Whiskers m(templ);
	m("a", "X")("b", list);
	BOOST_CHECK_THROW(m.render(), WhiskersError);
}

BOOST_AUTO_TEST_CASE(invalid_param)
{
	string templ = "a ";
	Whiskers m(templ);
	BOOST_CHECK_THROW(m("b ", "X"), WhiskersError);
}

BOOST_AUTO_TEST_CASE(invalid_param_rendered)
{
	string templ = "a ";
	Whiskers m(templ);
	BOOST_CHECK_EQUAL(m.render(), templ);
}

BOOST_AUTO_TEST_SUITE_END()

}


text:
/**
 * Copyright (c) 2017-present, Facebook, Inc. and its affiliates.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "logdevice/common/settings/Validators.h"

#include <boost/program_options.hpp>
#include <folly/String.h>
#include <sys/sysinfo.h>

#include "logdevice/common/commandline_util_chrono.h"

namespace facebook { namespace logdevice { namespace setting_validators {

/**
 * Compose an error message about the value of an option being out of range
 * and throw a boost::program_options::error with that message.
 */
void throw_invalid_value(const char* optname,
                         const char* expected,
                         const std::string& val) {
  char buf[4096];
  snprintf(buf,
           sizeof(buf),
           "invalid value for --%s: expected a %s value, got %s",
           optname,
           expected,
           val.c_str());
  throw boost::program_options::error(std::string(buf));
}

/**
 * Compose an error message about the value of an option being out of range
 * and throw a boost::program_options::error with that message.
 */
void throw_invalid_value(const char* optname,
                         const char* expected,
                         ssize_t num) {
  auto num_str = std::to_string(num);
  throw_invalid_value(optname, expected, num_str);
}

void validate_string_one_of(const char* optname,
                            const std::vector<std::string>& allowed,
                            const std::string& value) {
  if (std::find(allowed.begin(), allowed.end(), value) == allowed.end()) {
    char buf[4096];
    snprintf(buf,
             sizeof(buf),
             "invalid value for --%s: expected one of {\"%s\"}, got \"%s\"",
             optname,
             folly::join("\", \"", allowed).c_str(),
             value.c_str());
    throw boost::program_options::error(std::string(buf));
  }
}

void checksum_bits_notifier(int val) {
  if (val != 0 && val != 32 && val != 64) {
    throw boost::program_options::error(
        "Invalid value for --checksum-bits. Must be 0, 32 or 64.");
  }
}

size_t parse_memory_budget::getAvailableMemory() {
  struct sysinfo linux_sysinfo;
  int rv = sysinfo(&linux_sysinfo);
  // sysinfo cannot fail unless a bad pointer is passed.
  ld_check(rv == 0);
  return linux_sysinfo.totalram * linux_sysinfo.mem_unit;
}

size_t parse_memory_budget::operator()(const char* name,
                                       const std::string& value) {
  auto throw_invalid_value = [&]() {
    char buf[4096];
    snprintf(buf,
             sizeof(buf),
             "invalid value for --%s: expected a percentage in the form "
             "\"X%%\" where X is between ]0, 100] or a positive integer "
             "with an optional multipler (KMGT), got %s",
             name,
             value.c_str());
    throw boost::program_options::error(std::string(buf));
  };

  if (value.empty()) {
    throw_invalid_value();
  }

  // First, check if this is an absolute value in the form of an integer plus
  // optional multiplier.

  size_t val;
  if (parse_scaled_int(value.c_str(), &val) == 0) {
    if (val <= 0) {
      throw_invalid_value();
    }
    return val;
  }

  // Then, check if the value is a percentage value between 0 and 100.

  char* eptr;
  float percentage = strtof(value.c_str(), &eptr);

  if (eptr == value.c_str() || strncmp(eptr, "%", 2) != 0) {
    throw_invalid_value();
  }

  if (percentage <= 0 || percentage > 100) {
    throw_invalid_value();
  }

  return floor(percentage * getAvailableMemory() / 100.0);
}

void validate_unix_socket(const std::string& unix_socket) {
  // We require unix domain socket paths to be absolute paths.
  if (!unix_socket.empty() && unix_socket[0] != '/') {
    char buf[4096];
    snprintf(buf,
             sizeof buf,
             "invalid value for --unix-socket or --command-unix-socket: %s, "
             "must start with '/'",
             unix_socket.c_str());
    throw boost::program_options::error(buf);
  }
}

void validate_port(int port) {
  const int MIN_PORT = 1, MAX_PORT = 65535;
  if (port < MIN_PORT || port > MAX_PORT) {
    char buf[4096];
    snprintf(buf,
             sizeof buf,
             "invalid value for --port: %d, must be between %d and %d",
             port,
             MIN_PORT,
             MAX_PORT);
    throw boost::program_options::error(buf);
  }
}

void validate_optional_port(int port) {
  if (port == 0) {
    return;
  }
  validate_port(port);
}

}}} // namespace facebook::logdevice::setting_validators


text:
/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// mapnik
#include <mapnik/json/topology.hpp>
#include <mapnik/json/topojson_utils.hpp>
// stl
#include <string>
#include <vector>
#include <fstream>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/range/adaptor/reversed.hpp>
#pragma GCC diagnostic pop

#include "topojson_featureset.hpp"

topojson_featureset::topojson_featureset(mapnik::topojson::topology const& topo,
                                         mapnik::transcoder const& tr,
                                         array_type && index_array)
    : ctx_(std::make_shared<mapnik::context_type>()),
      topo_(topo),
      tr_(tr),
      index_array_(std::move(index_array)),
      index_itr_(index_array_.begin()),
      index_end_(index_array_.end()),
      feature_id_(1) {}

topojson_featureset::~topojson_featureset() {}

mapnik::feature_ptr topojson_featureset::next()
{
    if (index_itr_ != index_end_)
    {
        topojson_datasource::item_type const& item = *index_itr_++;
        std::size_t index = item.second;
        if ( index < topo_.geometries.size())
        {
            mapnik::topojson::geometry const& geom = topo_.geometries[index];
            mapnik::feature_ptr feature = mapnik::util::apply_visitor(
                mapnik::topojson::feature_generator<mapnik::context_ptr>(ctx_, tr_, topo_, feature_id_++),
                geom);
            return feature;
        }
    }

    return mapnik::feature_ptr();
}


text:
/**
 * @file ClusterManager.hpp
 * @brief Library header for Cluster Management
 * @author Sandeep D'souza
 *
 * Copyright (c) Carnegie Mellon University, 2016. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 	1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef CLUSTER_HPP
#define CLUSTER_HPP

// Boost includes
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/log/trivial.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

// Timeline Message
#include <msg/QoT_DCPS.hpp>

// std librasy includes
#include <vector>
#include <string>
#include <algorithm>
#include <mutex>
#include <condition_variable>

// Include the QoT Data Types
extern "C"
{
	#include "../../../qot_types.h"
}

// Get the Subscriber Entities
#include "MsgingEntities.cpp"

namespace qot
{
	// Distributed Inter-Process Cluster Management
	class ClusterManager
	{
		// Node Management Vectors
		// -> ClusterNodes: Specified Nodes in Coordination
		// -> ActiveNodes : Nodes that have joined the DDS Domain
		private: std::vector<std::string> ClusterNodes;
		private: std::vector<std::string> AliveNodes;

		// Constructor and destructor
		// The Constructor initializes private member variables and starts the DDS listener
		// The Destructor stops the DDS listener
		public: ClusterManager(const std::string &name, const std::string &uuid);
		public: ~ClusterManager();

		// Define the nodes which will be a part of the coordination
		public: qot_return_t DefineCluster(const std::vector<std::string> Nodes, qot_node_callback_t callback);

		// Wait for al the nodes to join the cluster
		public: qot_return_t WaitForReady();

		// Private Function -> Watch for changes to the cluster
		private: void watch();

		// Information about the application and the timeline
		private: std::string uuid;    // timeline uuid
		private: std::string name;    // application name

		// DDS Mesage Subscribing Entities
		private: qot::SubEntities sub_entity;

		// DDS NameService Message type
		private: qot_msgs::NameService name_msg;
		private: std::string filter_expression;

		// Handlers for Node Management
		//private: qot::NewUserHandler newUserHandler;
		//private: qot::UserLeftHandler userLeftHandler;

		// DDS Waitset
		private: dds::core::cond::WaitSet waitSet;

		// ClusterManagement Thread
		private: boost::thread thread;

		// Flag Signifying whether all  nodes have joined the DDS domain
		public: bool readyFlag;

		// Guard condition to terminate the watch thread
    	private: dds::core::cond::GuardCondition escape;

    	// Flag to terminate the cluster manager
		private: bool terminated;

		// ClusterManagement Ready Synchronization
		private: std::mutex mtx;
		private: std::condition_variable cv;

		// ClusterManagement User Callback
		private: qot_node_callback_t node_callback;

	};
}

#endif

ext: hpp

max_stars_repo_name: jpherbst/qot-stack

max_issues_repo_name: jpherbst/qot-stack

max_forks_repo_name: jpherbst/qot-stack


text:
#ifndef BOOST_THREAD_WIN32_MUTEX_HPP
#define BOOST_THREAD_WIN32_MUTEX_HPP
// (C) Copyright 2005-7 Anthony Williams
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include "basic_timed_mutex.hpp"
#include <boost/utility.hpp>
#include <boost/thread/exceptions.hpp>
#include <boost/thread/locks.hpp>

namespace boost
{
    namespace detail
    {
        typedef ::boost::detail::basic_timed_mutex underlying_mutex;
    }

    class mutex:
		boost::noncopyable,
        public ::boost::detail::underlying_mutex
    {
    public:
        mutex()
        {
            initialize();
        }
        ~mutex()
        {
            destroy();
        }

        typedef unique_lock<mutex> scoped_lock;
        typedef scoped_lock scoped_try_lock;
    };

    typedef mutex try_mutex;

    class timed_mutex:
        boost::noncopyable,
        public ::boost::detail::basic_timed_mutex
    {
    public:
        timed_mutex()
        {
            initialize();
        }

        ~timed_mutex()
        {
            destroy();
        }

        typedef unique_lock<timed_mutex> scoped_timed_lock;
        typedef scoped_timed_lock scoped_try_lock;
        typedef scoped_timed_lock scoped_lock;
    };
}

#endif


text:
#ifndef GRASP_QUALITY_H
#define GRASP_QUALITY_H

#include <vector>

#include <Eigen/Dense>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>

#include "reflex_interface/task_polytope.hpp"

class GraspWrenchSpace
{
public:
    std::vector<tf2::Vector3> force_primitives;
    std::vector<tf2::Vector3> torque_primitives;
};

class GraspQuality
{
public:
    enum ContactModel
    {
        PointContactWithoutFriction,
        HardFinger,
        SoftFinger
    };

    GraspQuality(float mu_epsilon = 1, float mu_delta = 1, int num_edges = 4, ContactModel contact_model = HardFinger);

    // all args have to be in same coordinate system!
    float getEpsilon(const std::vector<tf2::Vector3> &contact_positions,
                     const std::vector<tf2::Vector3> &contact_normals,
                     const tf2::Vector3 &object_com_world,
                     bool verbose = false);

    void fillEpsilonFTSeparate(const std::vector<tf2::Vector3> &contact_positions,
                               const std::vector<tf2::Vector3> &contact_normals,
                               const tf2::Vector3 &object_com_world,
                               float &epsilon_force,
                               float &epsilon_torque,
                               bool verbose = false);

    float getSlipMargin(std::vector<tf2::Vector3> &contact_normals,
                        const std::vector<tf2::Vector3> &contact_forces,
                        const std::vector<float> &contact_force_magnitudes,
                        const int &num_contacts,
                        bool verbose = false);

    template <class T>
    bool isSameSize(const int desired_size, const std::vector<T> &vector) { return int(vector.size()) == desired_size; };
    void updateGraspWrenchSpace(bool verbose);
    bool isValidNumContacts();
    Eigen::MatrixXd getCrossProductMatrix(const tf2::Vector3 &r);
    Eigen::MatrixXd getPartialGraspMatrix(const tf2::Transform &contact_frame, const tf2::Vector3 &object_position);
    Eigen::MatrixXd getGraspMatrix(const std::vector<tf2::Transform> &contact_frames,
                                   const tf2::Vector3 &object_position,
                                   const int &num_contacts);
    float getSlipMarginWithTaskWrenches(const TaskPolytope tp,
                                        std::vector<tf2::Vector3> &contact_forces,
                                        std::vector<tf2::Vector3> &contact_normals,
                                        const std::vector<tf2::Transform> &contact_frames,
                                        const tf2::Vector3 &object_position,
                                        const int &num_contacts);

private:
    float mu_epsilon;
    float mu_delta;
    int num_edges;
    int num_contacts;
    double beta;
    GraspWrenchSpace gws = GraspWrenchSpace();
    std::vector<tf2::Vector3> contact_positions;
    std::vector<tf2::Vector3> contact_normals;
    tf2::Vector3 object_com_world;
    ContactModel contact_model;
};

#endif


text:
// Copyright 2014-2015 SDL plc
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/** \file

    parse token sequence string into hypergraph (maybe with alignment features)
*/

#ifndef HYP__HYPERGRAPH_STRINGTOHYPERGRAPH_HPP
#define HYP__HYPERGRAPH_STRINGTOHYPERGRAPH_HPP
#pragma once

#include <sdl/Hypergraph/FeatureWeightUtil.hpp>
#include <sdl/Hypergraph/FeaturesPerInputPosition.hpp>
#include <sdl/Hypergraph/IHypergraph.hpp>
#include <sdl/Hypergraph/IsFeatureWeight.hpp>
#include <sdl/Hypergraph/MixFeature.hpp>
#include <sdl/Hypergraph/MutableHypergraph.hpp>
#include <sdl/Hypergraph/Tokens.hpp>
#include <sdl/Hypergraph/Types.hpp>
#include <sdl/Vocabulary/SpecialSymbols.hpp>
#include <sdl/Util/LogHelper.hpp>
#include <sdl/Util/Utf8.hpp>
#include <sdl/Exception.hpp>
#include <sdl/LexicalCast.hpp>
#include <sdl/SharedPtr.hpp>
#include <boost/noncopyable.hpp>
#include <cstddef>
#include <stdexcept>
#include <string>
#include <vector>

namespace sdl {
namespace Hypergraph {

/**
   Options for StringToHypergraph function; the inputFeatures
   object will be deleted on destruction.

struct UnkOptions {
  Sym terminalMaybeUnk(IVocabulary* pVoc, std::string const& token) const {
    if (doAddUnknownSymbols)
      return pVoc->addTerminal(token);
    else {
      Sym const sym = pVoc->terminal(token);
      return sym ? sym : UNK::ID;
    }
  }
  bool doAddUnknownSymbols = true;
};

 */
struct StringToHypergraphOptions {
  StringToHypergraphOptions() : inputFeatures(new NoFeatures()) {}

  StringToHypergraphOptions(IFeaturesPerInputPosition* newInputFeatures) : inputFeatures(newInputFeatures) {}

  explicit StringToHypergraphOptions(Hypergraph::TokensPtr const& tokens, IFeaturesPerInputPosition* feats)
      : tokens(tokens), inputFeatures(feats) {}

  Hypergraph::TokensPtr tokens;

  // Determines what features to put on what input position.
  shared_ptr<IFeaturesPerInputPosition> inputFeatures;
};

typedef std::vector<FeatureValue> TokenCosts;

struct TokenWeights {
  TokenWeights() : costs_() {}
  TokenWeights(FeatureValue const* costs, MixFeature<> const& mix) : costs_(costs), mixCost_(&mix) {}

  FeatureValue const* costs_;
  MixFeature<> const* mixCost_;

  template <class Weight>
  void reweight(Position i, Weight& weight) const {
    if (costs_) (*mixCost_)(costs_[i], weight);
  }
};

struct SymsFromStrings {
  IVocabulary* voc;
  span<std::string const> strings;

  SymsFromStrings(IVocabulary* voc, span<std::string const> const& strings) : voc(voc), strings(strings) {}
  Sym operator[](Position i) const { return voc->addTerminal(strings[i]); }
  Position size() const { return strings.size(); }
};

/**
   Converts a vector of strings, inputTokens to a flat-line hypergraph. Gives all
   non-lexical states lower state IDs than lexical states, which is
   what most algorithms need.

   \return number of tokens
 */
template <class Arc, class Syms>
Position symsToHypergraph(Syms const& inputTokens, IMutableHypergraph<Arc>* pHgResult,
                          StringToHypergraphOptions const& opts = StringToHypergraphOptions(),
                          TokenWeights const& inputWeights = TokenWeights()) {
  for (Position i = 0, numNonlexicalStates = inputTokens.size() + 1; i < numNonlexicalStates; ++i)
    pHgResult->addState();
  pHgResult->setStart(0);
  StateId prevState = 0;

  typedef typename Arc::Weight Weight;
  typedef FeatureInsertFct<Weight> FI;
  Position i = 0, n = inputTokens.size();
  for (; i < n; ++i) {
    const Sym sym = inputTokens[i];
    const StateId nextState = prevState + 1;
    Arc* pArc = new Arc(nextState, Tails(prevState, pHgResult->addState(sym)));
    Weight& weight = pArc->weight();
    assert(opts.inputFeatures != NULL);
    for (FeatureId featureId : opts.inputFeatures->getFeaturesForInputPosition(i)) {
      FI::insertNew(&weight, featureId, 1);
      if (opts.tokens) opts.tokens->insert(sym, featureId);
    }
    inputWeights.reweight(i, weight);
    pHgResult->addArc(pArc);
    prevState = nextState;
  }
  pHgResult->setFinal(prevState);
  return n;
}

template <class Arc, class Strings>
Position stringToHypergraph(Strings const& inputTokens, IMutableHypergraph<Arc>* pHgResult,
                            StringToHypergraphOptions const& opts = StringToHypergraphOptions(),
                            TokenWeights const& inputWeights = TokenWeights()) {
  return symsToHypergraph(SymsFromStrings(pHgResult->vocab(), inputTokens), pHgResult, opts, inputWeights);
}

/**
   take a utf8 string and break it into single-codepoint symbols, creating a straight-line hypergraph in
   \param pHgResult
*/
template <class Arc>
Position stringToHypergraph(std::string const& utf8string, IMutableHypergraph<Arc>* pHgResult,
                            StringToHypergraphOptions const& opts = StringToHypergraphOptions(),
                            TokenWeights const& inputWeights = TokenWeights()) {
  std::vector<std::string> utf8chars;
  Util::toUtf8Chs(utf8string, utf8chars);
  return stringToHypergraph(utf8chars, pHgResult, opts, inputWeights);
}

/**
   Creates a flat-line FST from two strings.
 */
template <class Arc, class Syms, class SymsOut>
void symsPairToFst(Syms const& inputTokens, SymsOut const& outputTokens, IMutableHypergraph<Arc>* pHgResult,
                   StringToHypergraphOptions const& opts = StringToHypergraphOptions()) {
  Position N = outputTokens.size();
  if (inputTokens.size() != N)
    SDL_THROW_LOG(Hypergraph.stringPairToFst, IndexException,
                  "The two strings must have same number of words");

  // 1. Create simple FSA from input tokens:
  symsToHypergraph(inputTokens, pHgResult, opts);

// 2. Insert output tokens:
#ifndef NDEBUG
  const StateId finalId = pHgResult->final();
#endif
  StateId stateId = pHgResult->start();
  for (Position i = 0; i < N; ++i) {
    Arc* arc = pHgResult->outArc(stateId, 0);
    assert(arc);
    setFsmOutputLabel(pHgResult, *arc, outputTokens[i]);
    stateId = arc->head();
  }
#ifndef NDEBUG
  assert(stateId == finalId);
#endif
}

template <class Arc, class Strings, class StringsOut>
void stringPairToFst(Strings const& inputTokens, StringsOut const& outputTokens,
                     IMutableHypergraph<Arc>* pHgResult,
                     StringToHypergraphOptions const& opts = StringToHypergraphOptions()) {
  IVocabulary* voc = pHgResult->vocab();
  symsPairToFst(SymsFromStrings(voc, inputTokens), SymsFromStrings(voc, outputTokens), pHgResult, opts);
}

}}

#endif

max_stars_repo_name: sdl-research/hyp

max_issues_repo_name: hypergraphs/hyp

max_forks_repo_name: hypergraphs/hyp