text:
#include "FragmentOptimizer/OptApp.h"
#include <pcl/console/print.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <boost/filesystem.hpp>

COptApp::COptApp(void)
    : resolution_(8), interval_(50), num_(0), weight_(1.0f), length_(3.0f),
      max_iteration_(5), max_inner_iteration_(10), ctr_filename_("output.ctr"),
      sample_filename_("sample.pcd"), pose_filename_("pose.log"),
      init_ctr_file_(""), dir_prefix_(""), sample_num_(-1),
      blacklist_pair_num_(10000) {}

COptApp::~COptApp(void) {}

void COptApp::InitMap() {
    absolute2relative_map_.resize(num_);
    relative2absolute_map_.clear();
    for (int i = 0; i < num_; i++) {
        if (blacklist_.find(i) == blacklist_.end()) {
            absolute2relative_map_[i] = relative2absolute_map_.size();
            relative2absolute_map_.push_back(i);
        } else {
            absolute2relative_map_[i] = -1;
        }
    }
    if (num_ != relative2absolute_map_.size()) {
        PCL_WARN("Blacklisted fragments, number reduced from %d to %d.\n", num_,
                 relative2absolute_map_.size());
    }
    num_ = relative2absolute_map_.size();
}

void COptApp::InitIPose() {
    if (ipose_.size() > 0) {
        return;
    }

    Eigen::Matrix4d basepose = Eigen::Matrix4d::Identity();
    basepose(0, 3) = length_ / 2.0;
    basepose(1, 3) = length_ / 2.0;
    basepose(2, 3) = -0.3;

    Eigen::Matrix4d baseinverse = basepose.inverse();

    Eigen::Matrix4d leftbase =
        basepose * rgbd_traj_.data_[0].transformation_.inverse();
    ipose_.resize(num_);
    pose_.resize(num_);
    pose_rot_t_.resize(num_);

    for (int i = 0; i < num_; i++) {
        int ii = relative2absolute_map_[i];
        ipose_[i] = leftbase *
                    rgbd_traj_.data_[ii * interval_].transformation_ *
                    baseinverse;
    }
    PCL_INFO("IPose initialized.\n");
}

void COptApp::InitPointClouds() {
    pointclouds_.clear();
    for (int i = 0; i < num_; i++) {
        pointclouds_.push_back(PointCloud(i, resolution_, length_));
    }

#pragma omp parallel for schedule(dynamic)
    for (int i = 0; i < num_; i++) {
        int ii = relative2absolute_map_[i];
        char filename[1024];
        memset(filename, 0, 1024);
        sprintf(filename, "%scloud_bin_%d.pcd", dir_prefix_.c_str(), ii);
        if (boost::filesystem::exists(std::string(filename))) {
            pointclouds_[i].LoadFromPCDFile(filename);
        } else {
            sprintf(filename, "%scloud_bin_xyzn_%d.xyzn", dir_prefix_.c_str(),
                    ii);
            if (boost::filesystem::exists(std::string(filename))) {
                pointclouds_[i].LoadFromXYZNFile(filename);
            } else {
                PCL_ERROR("File not found ... Check dir and num parameters.\n");
            }
        }
    }
}

void COptApp::InitCorrespondences() {
    corres_.clear();
    for (int i = 0; i < (int)reg_traj_.data_.size(); i++) {
        if (blacklist_.find(reg_traj_.data_[i].id1_) != blacklist_.end() ||
            blacklist_.find(reg_traj_.data_[i].id2_) != blacklist_.end() ||
            reg_traj_.data_[i].id1_ >= num_ ||
            reg_traj_.data_[i].id2_ >= num_) {
            continue;
        }
        if (reg_traj_.data_[i].frame_ != -1 &&
            reg_traj_.data_[i].frame_ >= blacklist_pair_num_) {
            corres_.push_back(Correspondence(
                absolute2relative_map_[reg_traj_.data_[i].id1_],
                absolute2relative_map_[reg_traj_.data_[i].id2_]));
            char filename[1024];
            memset(filename, 0, 1024);
            sprintf(filename, "%scorres_%d_%d.txt", dir_prefix_.c_str(),
                    reg_traj_.data_[i].id1_, reg_traj_.data_[i].id2_);
            corres_.back().trans_ = reg_traj_.data_[i].transformation_;
            corres_.back().LoadFromFile(filename);
            PCL_INFO("Read %s, get %d correspondences.\n", filename,
                     corres_.back().corres_.size());
        }
    }
}

void COptApp::OptimizeNonrigid() {
    PCL_WARN("Nonrigid optimization.\n");
    PCL_INFO("Parameters: weight %.5f, resolution %d, piece number %d, max "
             "iteration %d\n",
             weight_, resolution_, num_, max_iteration_);

    InitMap();
    unit_length_ = length_ / resolution_;
    nper_ = (resolution_ + 1) * (resolution_ + 1) * (resolution_ + 1) * 3;
    matrix_size_ = nper_ * num_;
    InitIPose();
    InitPointClouds();
    InitCorrespondences();

    Eigen::VectorXd ctr(matrix_size_);
    Eigen::VectorXd ictr(matrix_size_);
    Eigen::VectorXd oldctr(matrix_size_);
    Eigen::VectorXd Ab(matrix_size_);
    SparseMatrix baseAA(matrix_size_, matrix_size_);
    SparseMatrix thisAA(matrix_size_, matrix_size_);

    // init ctr
    InitCtr(ctr);
    ictr = ctr;
    oldctr = ctr;
    InitBaseAA(baseAA);

    double score;

    // iteration
    for (int itr = 0; itr < max_iteration_; itr++) {
        int nprocessed = 0;
        for (int l = 0; l < num_; l++) {
            pointclouds_[l].UpdateAllNormal(ctr);
        }

        thisAA = baseAA;

        PCL_INFO("Processing %d : %5d", corres_.size(), 0);

#pragma omp parallel for schedule(dynamic)
        for (int l = 0; l < (int)corres_.size(); l++) {
            int i = corres_[l].idx0_;
            int j = corres_[l].idx1_;
            TripletVector tri;
            HashSparseMatrix mati(i * nper_, i * nper_);
            HashSparseMatrix matj(j * nper_, j * nper_);
            HashSparseMatrix matij(i * nper_, j * nper_);

            int idx1[24], idx2[24];
            double val1[24], val2[24];

            for (int k = 0; k < (int)corres_[l].corres_.size(); k++) {
                int ii = corres_[l].corres_[k].first;
                int jj = corres_[l].corres_[k].second;
                Point &pi = pointclouds_[i].points_[ii];
                Point &pj = pointclouds_[j].points_[jj];

                for (int t = 0; t < 8; t++) {
                    idx1[t] = pi.idx_[t];
                    val1[t] = pi.val_[t] * weight_ * pi.n_[0];
                    idx1[8 + t] = pi.idx_[t] + 1;
                    val1[8 + t] = pi.val_[t] * weight_ * pi.n_[1];
                    idx1[16 + t] = pi.idx_[t] + 2;
                    val1[16 + t] = pi.val_[t] * weight_ * pi.n_[2];
                    idx2[t] = pj.idx_[t];
                    val2[t] = -pj.val_[t] * weight_ * pi.n_[0];
                    idx2[8 + t] = pj.idx_[t] + 1;
                    val2[8 + t] = -pj.val_[t] * weight_ * pi.n_[1];
                    idx2[16 + t] = pj.idx_[t] + 2;
                    val2[16 + t] = -pj.val_[t] * weight_ * pi.n_[2];
                }

                mati.AddHessian(idx1, val1, 24, tri);
                matj.AddHessian(idx2, val2, 24, tri);
                matij.AddHessian(idx1, val1, 24, idx2, val2, 24, tri);
            }

            SparseMatrix tempAA(matrix_size_, matrix_size_);
            tempAA.setFromTriplets(tri.begin(), tri.end());

#pragma omp critical
            {
                nprocessed++;
                thisAA += tempAA;
                PCL_INFO("\b\b\b\b\b%5d", nprocessed);
            }
        }
        PCL_INFO(" ... Done.\n");

        Eigen::CholmodSupernodalLLT<SparseMatrix, Eigen::Upper> solver;
        solver.analyzePattern(thisAA);
        solver.factorize(thisAA);

        if (sample_num_ > 0) {
            char filename[1024];
            memset(filename, 0, 1024);
            sprintf(filename, "itr%d.ctr", itr);
            SaveCtr(oldctr, filename);
        }

        for (int m = 0; m < max_inner_iteration_; m++) {
            Ab.setZero();
            for (int l = 0; l < num_; l++) {
                for (int i = 0; i <= resolution_; i++) {
                    for (int j = 0; j <= resolution_; j++) {
                        for (int k = 0; k <= resolution_; k++) {
                            int idx = GetIndex(i, j, k) * 3 + l * nper_;
                            std::vector<int> idxx;
                            if (i > 0) {
                                idxx.push_back(GetIndex(i - 1, j, k) * 3 +
                                               l * nper_);
                            }
                            if (i < resolution_) {
                                idxx.push_back(GetIndex(i + 1, j, k) * 3 +
                                               l * nper_);
                            }
                            if (j > 0) {
                                idxx.push_back(GetIndex(i, j - 1, k) * 3 +
                                               l * nper_);
                            }
                            if (j < resolution_) {
                                idxx.push_back(GetIndex(i, j + 1, k) * 3 +
                                               l * nper_);
                            }
                            if (k > 0) {
                                idxx.push_back(GetIndex(i, j, k - 1) * 3 +
                                               l * nper_);
                            }
                            if (k < resolution_) {
                                idxx.push_back(GetIndex(i, j, k + 1) * 3 +
                                               l * nper_);
                            }
                            Eigen::Matrix3d R =
                                GetRotation(idx, idxx, ictr, ctr);
                            for (int t = 0; t < (int)idxx.size(); t++) {
                                Eigen::Vector3d bx =
                                    R * Eigen::Vector3d(
                                            ictr(idx) - ictr(idxx[t]),
                                            ictr(idx + 1) - ictr(idxx[t] + 1),
                                            ictr(idx + 2) - ictr(idxx[t] + 2));
                                Ab(idx) += bx(0);
                                Ab(idxx[t]) -= bx(0);
                                Ab(idx + 1) += bx(1);
                                Ab(idxx[t] + 1) -= bx(1);
                                Ab(idx + 2) += bx(2);
                                Ab(idxx[t] + 2) -= bx(2);
                            }
                        }
                    }
                }
            }

            oldctr = ctr;
            ctr = solver.solve(Ab);

            score = (oldctr - ctr).norm();
            PCL_WARN("Iteration #%d:%d (%d:%d) : score is %.4f\n", itr + 1,
                     m + 1, max_iteration_, max_inner_iteration_, score);

            if (sample_num_ > 0) {
                char filename[1024];
                memset(filename, 0, 1024);
                sprintf(filename, "itr%d_inner%d_out.ctr", itr, m);
                SaveCtr(ctr, filename);
            }
        }
    }

    SaveCtr(ctr, ctr_filename_);
    SavePoints(ctr, sample_filename_);
}

void COptApp::OptimizeRigid() {
    PCL_WARN("Rigid optimization.\n");
    PCL_INFO("Parameters: resolution %d, piece number %d, max iteration %d\n",
             resolution_, num_, max_iteration_);

    InitMap();
    unit_length_ = length_ / resolution_;
    nper_ = (resolution_ + 1) * (resolution_ + 1) * (resolution_ + 1) * 3;
    matrix_size_ = 6 * num_; // rigid only
    InitIPose();
    InitPointClouds();
    InitCorrespondences();

    rgbd_traj_.data_.clear();
    for (int i = 0; i < (int)ipose_.size(); i++) {
        rgbd_traj_.data_.push_back(
            FramedTransformation(i, i, i + 1, ipose_[i]));
        pose_[i] = ipose_[i];
        pointclouds_[i].UpdatePose(pose_[i].cast<float>());
    }

    for (int itr = 0; itr < max_iteration_; itr++) {
        Eigen::VectorXd thisJb(matrix_size_);
        SparseMatrix thisJJ(matrix_size_, matrix_size_);

        thisJb.setZero();
        thisJJ.setZero();

        double thisscore = 0.0;
        int nprocessed = 0;

        PCL_INFO("Processing %d : %5d", corres_.size(), 0);

#pragma omp parallel for schedule(dynamic)
        for (int l = 0; l < (int)corres_.size(); l++) {
            int i = corres_[l].idx0_;
            int j = corres_[l].idx1_;
            TripletVector tri;
            HashSparseMatrix mat_adder(0, 0);
            Eigen::VectorXd tempJb(matrix_size_);
            SparseMatrix tempJJ(matrix_size_, matrix_size_);
            tempJb.setZero();
            tempJJ.setZero();

            for (int k = 0; k < 6; k++) {
                mat_adder.Add(k, k, 1, tri);
            }

            int idx[12];
            double val[12];
            double b;
            double score = 0.0;

            for (int k = 0; k < (int)corres_[l].corres_.size(); k++) {
                int ii = corres_[l].corres_[k].first;
                int jj = corres_[l].corres_[k].second;
                Point &pi = pointclouds_[i].points_[ii];
                Point &pj = pointclouds_[j].points_[jj];
                Eigen::Vector4d ppi(pi.p_[0], pi.p_[1], pi.p_[2], 1.0);
                Eigen::Vector4d ppj(pj.p_[0], pj.p_[1], pj.p_[2], 1.0);
                Eigen::Vector4d npi(pi.n_[0], pi.n_[1], pi.n_[2], 0.0);
                Eigen::Vector4d npj(pj.n_[0], pj.n_[1], pj.n_[2], 0.0);

                b = (ppi - ppj).dot(npi);
                score += b * b;

                idx[0] = i * 6;
                idx[1] = i * 6 + 1;
                idx[2] = i * 6 + 2;
                idx[3] = i * 6 + 3;
                idx[4] = i * 6 + 4;
                idx[5] = i * 6 + 5;
                idx[6] = j * 6;
                idx[7] = j * 6 + 1;
                idx[8] = j * 6 + 2;
                idx[9] = j * 6 + 3;
                idx[10] = j * 6 + 4;
                idx[11] = j * 6 + 5;

                val[0] = Eigen::Vector4d(0, -ppi(2), ppi(1), 1).dot(npi) +
                         Eigen::Vector4d(0, -npi(2), npi(1), 0).dot(ppi - ppj);
                val[1] = Eigen::Vector4d(ppi(2), 0, -ppi(0), 1).dot(npi) +
                         Eigen::Vector4d(npi(2), 0, -npi(0), 0).dot(ppi - ppj);
                val[2] = Eigen::Vector4d(-ppi(1), ppi(0), 0, 1).dot(npi) +
                         Eigen::Vector4d(-npi(1), npi(0), 0, 0).dot(ppi - ppj);
                val[3] = npi(0);
                val[4] = npi(1);
                val[5] = npi(2);
                val[6] = -Eigen::Vector4d(0, -ppj(2), ppj(1), 1).dot(npi);
                val[7] = -Eigen::Vector4d(ppj(2), 0, -ppj(0), 1).dot(npi);
                val[8] = -Eigen::Vector4d(-ppj(1), ppj(0), 0, 1).dot(npi);
                val[9] = -npi(0);
                val[10] = -npi(1);
                val[11] = -npi(2);

                mat_adder.AddHessian(idx, val, 12, tri);
                mat_adder.AddJb(idx, val, 12, b, tempJb);
            }

            tempJJ.setFromTriplets(tri.begin(), tri.end());

#pragma omp critical
            {
                nprocessed++;
                thisscore += score;
                thisJJ += tempJJ;
                thisJb += tempJb;
                PCL_INFO("\b\b\b\b\b%5d", nprocessed);
            }
        }
        PCL_INFO(" ... Done.\n");
        PCL_INFO("Error score is : %.2f\n", thisscore);

        Eigen::CholmodSupernodalLLT<SparseMatrix, Eigen::Upper> solver;
        solver.analyzePattern(thisJJ);
        solver.factorize(thisJJ);

        Eigen::VectorXd result = -solver.solve(thisJb);

        for (int l = 0; l < num_; l++) {
            Eigen::Affine3d aff_mat;
            aff_mat.linear() =
                (Eigen::Matrix3d)Eigen::AngleAxisd(result(l * 6 + 2),
                                                   Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(result(l * 6 + 1), Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(result(l * 6 + 0), Eigen::Vector3d::UnitX());
            aff_mat.translation() = Eigen::Vector3d(
                result(l * 6 + 3), result(l * 6 + 4), result(l * 6 + 5));
            pose_[l] = aff_mat.matrix() * pose_[l];
            pointclouds_[l].UpdatePose(aff_mat.matrix().cast<float>());
        }
    }

    output_traj_.data_.clear();
    for (int i = 0; i < (int)pose_.size(); i++) {
        output_traj_.data_.push_back(
            FramedTransformation(i, i, i + 1, pose_[i]));
    }
    output_traj_.SaveToFile(pose_filename_);

    Eigen::VectorXd ctr(nper_ * num_);
    Pose2Ctr(pose_, ctr);
    SaveCtr(ctr, ctr_filename_);
    SavePoints(ctr, sample_filename_);
}

void COptApp::OptimizeSLAC() {
    PCL_WARN("SLAC optimization.\n");
    PCL_INFO("Parameters: weight %.5f, resolution %d, piece number %d, max "
             "iteration %d\n",
             weight_, resolution_, num_, max_iteration_);

    double default_weight = num_ * weight_;

    InitMap();
    unit_length_ = length_ / resolution_;
    nper_ = (resolution_ + 1) * (resolution_ + 1) * (resolution_ + 1) * 3;
    matrix_size_ = 6 * num_ + nper_;
    InitIPose();
    InitPointClouds();
    InitCorrespondences();

    Eigen::VectorXd ictr(matrix_size_);
    Eigen::VectorXd thisCtr(matrix_size_);
    InitCtrSLAC(ictr, thisCtr);
    Eigen::VectorXd expand_ctr(nper_ * num_);

    rgbd_traj_.data_.clear();
    for (int i = 0; i < (int)ipose_.size(); i++) {
        rgbd_traj_.data_.push_back(
            FramedTransformation(i, i, i + 1, ipose_[i]));
        pose_[i] = ipose_[i];
        pose_rot_t_[i] = pose_[i].block<3, 3>(0, 0).transpose();
        pointclouds_[i].UpdatePose(pose_[i].cast<float>());
    }

    {
        pcl::ScopeTime ttime("Neat Optimization");

        SparseMatrix baseJJ(matrix_size_, matrix_size_);
        InitBaseJJ(baseJJ);

        baseJJ *= default_weight;

        for (int itr = 0; itr < max_iteration_; itr++) {
            pcl::ScopeTime time(
                ("SLAC iteration " + std::to_string(itr)).c_str());
            Eigen::VectorXd thisJb(matrix_size_);
            Eigen::MatrixXd thisJJ(baseJJ.triangularView<Eigen::Upper>());
            Eigen::VectorXd result;

            thisJJ(0, 0) += 1.0;
            thisJJ(1, 1) += 1.0;
            thisJJ(2, 2) += 1.0;
            thisJJ(3, 3) += 1.0;
            thisJJ(4, 4) += 1.0;
            thisJJ(5, 5) += 1.0;

            thisJb.setZero();

            double thisscore = 0.0;
            int nprocessed = 0;

            PCL_INFO("Processing %d : %5d", corres_.size(), 0);

#pragma omp parallel for schedule(dynamic)
            for (int l = 0; l < (int)corres_.size(); l++) {
                int i = corres_[l].idx0_;
                int j = corres_[l].idx1_;
                const int buck_size = 12 + 24 * 2;
                int idx[buck_size];
                double val[buck_size];
                double b;
                double score = 0.0;
                Eigen::MatrixXd tempJJ(matrix_size_, matrix_size_);
                tempJJ.setZero();
                Eigen::VectorXd tempJb(matrix_size_);
                tempJb.setZero();

                for (int k = 0; k < (int)corres_[l].corres_.size(); k++) {
                    int ii = corres_[l].corres_[k].first;
                    int jj = corres_[l].corres_[k].second;
                    Point &pi = pointclouds_[i].points_[ii];
                    Point &pj = pointclouds_[j].points_[jj];
                    Eigen::Vector3d ppi(pi.p_[0], pi.p_[1], pi.p_[2]);
                    Eigen::Vector3d ppj(pj.p_[0], pj.p_[1], pj.p_[2]);
                    Eigen::Vector3d npi(pi.n_[0], pi.n_[1], pi.n_[2]);
                    Eigen::Vector3d npj(pj.n_[0], pj.n_[1], pj.n_[2]);
                    Eigen::Vector3d diff = ppi - ppj;
                    b = diff.dot(npi);
                    score += b * b;

                    idx[0] = i * 6;
                    idx[1] = i * 6 + 1;
                    idx[2] = i * 6 + 2;
                    idx[3] = i * 6 + 3;
                    idx[4] = i * 6 + 4;
                    idx[5] = i * 6 + 5;
                    idx[6] = j * 6;
                    idx[7] = j * 6 + 1;
                    idx[8] = j * 6 + 2;
                    idx[9] = j * 6 + 3;
                    idx[10] = j * 6 + 4;
                    idx[11] = j * 6 + 5;

                    Eigen::Vector3d temp = ppj.cross(npi);

                    val[0] = temp(0);
                    val[1] = temp(1);
                    val[2] = temp(2);
                    val[3] = npi(0);
                    val[4] = npi(1);
                    val[5] = npi(2);
                    val[6] = -temp(0);
                    val[7] = -temp(1);
                    val[8] = -temp(2);
                    val[9] = -npi(0);
                    val[10] = -npi(1);
                    val[11] = -npi(2);

                    // next part of Jacobian
                    // deal with control lattices
                    Eigen::Vector3d dTi = pose_rot_t_[i] * npi;
                    Eigen::Vector3d dTj = -pose_rot_t_[j] * npi;

                    for (int ll = 0; ll < 8; ll++) {
                        for (int xyz = 0; xyz < 3; xyz++) {
                            idx[12 + ll * 3 + xyz] =
                                6 * num_ + pi.idx_[ll] + xyz;
                            val[12 + ll * 3 + xyz] = pi.val_[ll] * dTi(xyz);
                        }
                    }

                    for (int ll = 0; ll < 8; ll++) {
                        for (int xyz = 0; xyz < 3; xyz++) {
                            idx[12 + 24 + ll * 3 + xyz] =
                                6 * num_ + pj.idx_[ll] + xyz;
                            val[12 + 24 + ll * 3 + xyz] =
                                pj.val_[ll] * dTj(xyz);
                        }
                    }

                    for (int i = 0; i < buck_size; i++) {
                        tempJJ(idx[i], idx[i]) += val[i] * val[i];
                        for (int j = i + 1; j < buck_size; j++) {
                            if (idx[i] == idx[j]) {
                                tempJJ(idx[i], idx[j]) += 2 * val[i] * val[j];
                            } else if (idx[i] < idx[j]) {
                                tempJJ(idx[i], idx[j]) += val[i] * val[j];
                            } else {
                                tempJJ(idx[j], idx[i]) += val[i] * val[j];
                            }
                        }
                        tempJb(idx[i]) += b * val[i];
                    }
                }

#pragma omp critical
                {
                    nprocessed++;
                    thisscore += score;
                    thisJJ += tempJJ;
                    thisJb += tempJb;
                    PCL_INFO("\b\b\b\b\b%5d", nprocessed);
                }
            }
            PCL_INFO(" ... Done.\n");
            PCL_INFO("Data error score is : %.2f\n", thisscore);

            SparseMatrix thisSparseJJ = thisJJ.sparseView();
            Eigen::CholmodSupernodalLLT<SparseMatrix, Eigen::Upper> solver;
            solver.analyzePattern(thisSparseJJ);
            solver.factorize(thisSparseJJ);

            Eigen::VectorXd tempCtr(thisCtr);

            // regularizer

            Eigen::VectorXd dataJb(thisJb);

            Eigen::VectorXd baseJb(matrix_size_);
            baseJb.setZero();

            double regscore = 0.0;
            for (int i = 0; i <= resolution_; i++) {
                for (int j = 0; j <= resolution_; j++) {
                    for (int k = 0; k <= resolution_; k++) {
                        int idx = GetIndex(i, j, k) * 3 + 6 * num_;
                        std::vector<int> idxx;
                        if (i > 0) {
                            idxx.push_back(GetIndex(i - 1, j, k) * 3 +
                                           6 * num_);
                        }
                        if (i < resolution_) {
                            idxx.push_back(GetIndex(i + 1, j, k) * 3 +
                                           6 * num_);
                        }
                        if (j > 0) {
                            idxx.push_back(GetIndex(i, j - 1, k) * 3 +
                                           6 * num_);
                        }
                        if (j < resolution_) {
                            idxx.push_back(GetIndex(i, j + 1, k) * 3 +
                                           6 * num_);
                        }
                        if (k > 0) {
                            idxx.push_back(GetIndex(i, j, k - 1) * 3 +
                                           6 * num_);
                        }
                        if (k < resolution_) {
                            idxx.push_back(GetIndex(i, j, k + 1) * 3 +
                                           6 * num_);
                        }

                        Eigen::Matrix3d R;
                        if (i == resolution_ / 2 && j == resolution_ / 2 &&
                            k == 0) {
                            // anchor point, Rotation matrix is always identity
                            R = R.Identity();
                        } else {
                            R = GetRotation(idx, idxx, ictr, tempCtr);
                        }

                        for (int t = 0; t < (int)idxx.size(); t++) {
                            Eigen::Vector3d bx =
                                Eigen::Vector3d(
                                    tempCtr(idx) - tempCtr(idxx[t]),
                                    tempCtr(idx + 1) - tempCtr(idxx[t] + 1),
                                    tempCtr(idx + 2) - tempCtr(idxx[t] + 2)) -
                                R * Eigen::Vector3d(
                                        ictr(idx) - ictr(idxx[t]),
                                        ictr(idx + 1) - ictr(idxx[t] + 1),
                                        ictr(idx + 2) - ictr(idxx[t] + 2));
                            regscore += default_weight * bx.transpose() * bx;
                            baseJb(idx) += bx(0) * default_weight;
                            baseJb(idxx[t]) -= bx(0) * default_weight;
                            baseJb(idx + 1) += bx(1) * default_weight;
                            baseJb(idxx[t] + 1) -= bx(1) * default_weight;
                            baseJb(idx + 2) += bx(2) * default_weight;
                            baseJb(idxx[t] + 2) -= bx(2) * default_weight;
                        }
                    }
                }
            }
            thisJb = dataJb + baseJb;

            PCL_INFO("Regularization error score is : %.2f\n", regscore);

            result = -solver.solve(thisJb);

            for (int l = 0; l < nper_; l++) {
                tempCtr(num_ * 6 + l) =
                    thisCtr(num_ * 6 + l) + result(num_ * 6 + l);
            }

            for (int l = 0; l < nper_; l++) {
                thisCtr(num_ * 6 + l) += result(num_ * 6 + l);
            }

            for (int l = 0; l < num_; l++) {
                Eigen::Affine3d aff_mat;
                aff_mat.linear() =
                    (Eigen::Matrix3d)Eigen::AngleAxisd(
                        result(l * 6 + 2), Eigen::Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(result(l * 6 + 1),
                                      Eigen::Vector3d::UnitY()) *
                    Eigen::AngleAxisd(result(l * 6 + 0),
                                      Eigen::Vector3d::UnitX());
                aff_mat.translation() = Eigen::Vector3d(
                    result(l * 6 + 3), result(l * 6 + 4), result(l * 6 + 5));
                // std::cout << aff_mat.matrix() << endl << std::endl;
                // update
                pose_[l] = aff_mat.matrix() * pose_[l];
                pose_rot_t_[l] = pose_[l].block<3, 3>(0, 0).transpose();
            }
            ExpandCtr(thisCtr, expand_ctr);
            for (int l = 0; l < num_; l++) {
                pointclouds_[l].UpdateAllPointPN(expand_ctr);
            }
        }
    }

    output_traj_.data_.clear();
    for (int i = 0; i < (int)pose_.size(); i++) {
        output_traj_.data_.push_back(
            FramedTransformation(i, i, i + 1, pose_[i]));
    }
    output_traj_.SaveToFile(pose_filename_);

    ExpandCtr(thisCtr, expand_ctr);
    SaveCtr(expand_ctr, ctr_filename_);
    SavePoints(expand_ctr, sample_filename_);
}

void COptApp::InitCtr(Eigen::VectorXd &ctr) {
    if (init_ctr_file_.length() > 1) {
        FILE *f = fopen(init_ctr_file_.c_str(), "r");
        if (f != NULL) {
            char buffer[1024];
            for (int i = 0; i < num_ * nper_ / 3; i++) {
                fgets(buffer, 1024, f);
                sscanf(buffer, "%lf %lf %lf", &ctr(i * 3), &ctr(i * 3 + 1),
                       &ctr(i * 3 + 2));
            }
            fclose(f);
        }
    } else {
        for (int l = 0; l < num_; l++) {
            for (int i = 0; i <= resolution_; i++) {
                for (int j = 0; j <= resolution_; j++) {
                    for (int k = 0; k <= resolution_; k++) {
                        Eigen::Vector4d pos(i * unit_length_, j * unit_length_,
                                            k * unit_length_, 1);
                        Eigen::Vector4d ppos = ipose_[l] * pos;
                        ctr(l * nper_ + GetIndex(i, j, k) * 3 + 0) = ppos(0);
                        ctr(l * nper_ + GetIndex(i, j, k) * 3 + 1) = ppos(1);
                        ctr(l * nper_ + GetIndex(i, j, k) * 3 + 2) = ppos(2);
                    }
                }
            }
        }
    }
}

void COptApp::InitCtrSLAC(Eigen::VectorXd &ctr, Eigen::VectorXd &thisCtr) {
    for (int i = 0; i <= resolution_; i++) {
        for (int j = 0; j <= resolution_; j++) {
            for (int k = 0; k <= resolution_; k++) {
                Eigen::Vector4d pos(i * unit_length_, j * unit_length_,
                                    k * unit_length_, 1);
                ctr(num_ * 6 + GetIndex(i, j, k) * 3 + 0) = pos(0);
                ctr(num_ * 6 + GetIndex(i, j, k) * 3 + 1) = pos(1);
                ctr(num_ * 6 + GetIndex(i, j, k) * 3 + 2) = pos(2);
            }
        }
    }
    if (init_ctr_file_.length() > 1) {
        FILE *f = fopen(init_ctr_file_.c_str(), "r");
        if (f != NULL) {
            char buffer[1024];
            for (int i = 0; i < nper_ / 3; i++) {
                fgets(buffer, 1024, f);
                sscanf(buffer, "%lf %lf %lf", &thisCtr(num_ * 6 + i * 3),
                       &thisCtr(num_ * 6 + i * 3 + 1),
                       &thisCtr(num_ * 6 + i * 3 + 2));
            }
            fclose(f);
        }
    } else {
        thisCtr = ctr;
    }
}

void COptApp::Pose2Ctr(std::vector<Eigen::Matrix4d> &pose,
                       Eigen::VectorXd &ctr) {
    for (int l = 0; l < num_; l++) {
        for (int i = 0; i <= resolution_; i++) {
            for (int j = 0; j <= resolution_; j++) {
                for (int k = 0; k <= resolution_; k++) {
                    Eigen::Vector4d pos(i * unit_length_, j * unit_length_,
                                        k * unit_length_, 1);
                    Eigen::Vector4d ppos = pose[l] * pos;
                    ctr(l * nper_ + GetIndex(i, j, k) * 3 + 0) = ppos(0);
                    ctr(l * nper_ + GetIndex(i, j, k) * 3 + 1) = ppos(1);
                    ctr(l * nper_ + GetIndex(i, j, k) * 3 + 2) = ppos(2);
                }
            }
        }
    }
}

void COptApp::ExpandCtr(const Eigen::VectorXd &ctr,
                        Eigen::VectorXd &expand_ctr) {
    expand_ctr.setZero();
    for (int l = 0; l < num_; l++) {
        for (int i = 0; i < nper_ / 3; i++) {
            Eigen::Vector4d pos =
                pose_[l] * Eigen::Vector4d(ctr(num_ * 6 + i * 3 + 0),
                                           ctr(num_ * 6 + i * 3 + 1),
                                           ctr(num_ * 6 + i * 3 + 2), 1);
            expand_ctr(l * nper_ + i * 3 + 0) = pos(0);
            expand_ctr(l * nper_ + i * 3 + 1) = pos(1);
            expand_ctr(l * nper_ + i * 3 + 2) = pos(2);
        }
    }
}

void COptApp::InitBaseAA(SparseMatrix &baseAA) {
    TripletVector tri;

    for (int l = 0; l < num_; l++) {
        HashSparseMatrix mat(l * nper_, l * nper_);
        for (int i = 0; i <= resolution_; i++) {
            for (int j = 0; j <= resolution_; j++) {
                for (int k = 0; k <= resolution_; k++) {
                    int idx[2] = {GetIndex(i, j, k) * 3, 0};
                    double val[2] = {1, -1};
                    if (i > 0) {
                        idx[1] = GetIndex(i - 1, j, k) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                    if (i < resolution_) {
                        idx[1] = GetIndex(i + 1, j, k) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                    if (j > 0) {
                        idx[1] = GetIndex(i, j - 1, k) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                    if (j < resolution_) {
                        idx[1] = GetIndex(i, j + 1, k) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                    if (k > 0) {
                        idx[1] = GetIndex(i, j, k - 1) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                    if (k < resolution_) {
                        idx[1] = GetIndex(i, j, k + 1) * 3;
                        mat.AddHessian2(idx, val, tri);
                    }
                }
            }
        }
        if (l == 0) {
            mat.Add(0, 0, 1, tri);
            mat.Add(1, 1, 1, tri);
            mat.Add(2, 2, 1, tri);
        }
    }

    baseAA.setFromTriplets(tri.begin(), tri.end());
}

void COptApp::InitBaseJJ(SparseMatrix &baseJJ) {
    TripletVector tri;

    HashSparseMatrix mat(6 * num_, 6 * num_);
    for (int i = 0; i <= resolution_; i++) {
        for (int j = 0; j <= resolution_; j++) {
            for (int k = 0; k <= resolution_; k++) {
                int idx[2] = {GetIndex(i, j, k) * 3, 0};
                double val[2] = {1, -1};
                if (i > 0) {
                    idx[1] = GetIndex(i - 1, j, k) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
                if (i < resolution_) {
                    idx[1] = GetIndex(i + 1, j, k) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
                if (j > 0) {
                    idx[1] = GetIndex(i, j - 1, k) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
                if (j < resolution_) {
                    idx[1] = GetIndex(i, j + 1, k) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
                if (k > 0) {
                    idx[1] = GetIndex(i, j, k - 1) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
                if (k < resolution_) {
                    idx[1] = GetIndex(i, j, k + 1) * 3;
                    mat.AddHessian2(idx, val, tri);
                }
            }
        }
    }

    int base_anchor = GetIndex(resolution_ / 2, resolution_ / 2, 0) * 3;

    mat.Add(base_anchor + 0, base_anchor + 0, 1, tri);
    mat.Add(base_anchor + 1, base_anchor + 1, 1, tri);
    mat.Add(base_anchor + 2, base_anchor + 2, 1, tri);

    baseJJ.setFromTriplets(tri.begin(), tri.end());
}

Eigen::Matrix3d COptApp::GetRotation(const int idx,
                                     const std::vector<int> &idxx,
                                     const Eigen::VectorXd &ictr,
                                     const Eigen::VectorXd &ctr) {
    int n = (int)idxx.size();
    Eigen::Matrix3d C = Eigen::Matrix3d::Zero();

    for (int i = 0; i < n; i++) {
        Eigen::RowVector3d dif(ictr(idx) - ictr(idxx[i]),
                               ictr(idx + 1) - ictr(idxx[i] + 1),
                               ictr(idx + 2) - ictr(idxx[i] + 2));
        Eigen::RowVector3d diff(ctr(idx) - ctr(idxx[i]),
                                ctr(idx + 1) - ctr(idxx[i] + 1),
                                ctr(idx + 2) - ctr(idxx[i] + 2));
        C += dif.transpose() * diff;
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(C, Eigen::ComputeFullU |
                                                 Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::Matrix3d R = V * U.transpose();
    if (R.determinant() < 0) {
        U(0, 2) *= -1;
        U(1, 2) *= -1;
        U(2, 2) *= -1;
        R = V * U.transpose();
    }
    return R;
}

void COptApp::SaveCtr(const Eigen::VectorXd &ctr, std::string filename) {
    PCL_INFO("Save ctr to file %s ... ", filename.c_str());
    FILE *f = fopen(filename.c_str(), "w");
    if (f != NULL) {
        for (int i = 0; i < num_ * nper_ / 3; i++) {
            fprintf(f, "%.10f %.10f %.10f\n", ctr(i * 3), ctr(i * 3 + 1),
                    ctr(i * 3 + 2));
        }
        fclose(f);
    }
    PCL_INFO("Done.\n");
}

void COptApp::SavePoints(const Eigen::VectorXd &ctr, std::string filename) {
    if (sample_num_ <= 0)
        return;

    pcl::PointCloud<pcl::PointXYZRGBNormal> pcd;
    pcl::PointXYZRGBNormal pt;
    for (int l = 0; l < num_; l++) {
        pointclouds_[l].UpdateAllNormal(ctr);
        for (int i = 0; i < (int)pointclouds_[l].points_.size();
             i += sample_num_) {
            Eigen::Vector3d pos =
                pointclouds_[l].UpdatePoint(ctr, pointclouds_[l].points_[i]);
            Eigen::Vector3d norm(pointclouds_[l].points_[i].n_[0],
                                 pointclouds_[l].points_[i].n_[1],
                                 pointclouds_[l].points_[i].n_[2]);
            norm.normalize();
            pt.x = pos(0);
            pt.y = pos(1);
            pt.z = pos(2);
            pt.normal_x = norm(0);
            pt.normal_y = norm(1);
            pt.normal_z = norm(2);
            pcd.push_back(pt);
        }
    }

    PCL_INFO("Save sample pcd into %s ... ", filename.c_str());
    pcl::PCDWriter w;
    w.writeBinaryCompressed(filename, pcd);
}

void COptApp::Blacklist(std::string filename) {
    blacklist_.clear();

    FILE *f = fopen(filename.c_str(), "r");
    if (f != NULL) {
        char buffer[1024];
        int id;
        while (fgets(buffer, 1024, f) != NULL) {
            if (strlen(buffer) > 0 && buffer[0] != '#') {
                sscanf(buffer, "%d", &id);
                blacklist_.insert(id);
            }
        }
        fclose(f);
    }
}

max_stars_repo_name: yxlao/ElasticReconstruction

max_issues_repo_name: yxlao/ElasticReconstruction

max_forks_repo_name: yxlao/ElasticReconstruction


text:
#include <string>
#include <vector>
#include <memory>
#include <cassert>
#include <algorithm>
#include <gflags/gflags.h>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>
#include "file_util.h"
#include "time_util.h"
#include "classifier.h"

using namespace std;
using namespace boost;
using test::tensorrt::ReadFileByLine;
using test::tensorrt::WriteFileByLine;
using test::tensorrt::GetTimeMilli;
using test::tensorrt::Classifier;

DEFINE_string(dataset_dir, "", "test dataset directory");
DEFINE_string(gt_text, "", "ground truth text file path");
DEFINE_string(output_path, "", "output file path");
DEFINE_bool(verbose, true, "whether to print some info");
DEFINE_string(engine_file, "", "trt engine's path");
DEFINE_string(output_blob_file, "", "model output blob names path");
DEFINE_string(mean_file, "", "mean file's path");
DEFINE_string(label_file, "", "class labels text file");
DEFINE_double(stdvar, 1.0, "input standard variance");
DEFINE_int32(base_size, 0, "resize base size before cropped");
DEFINE_int32(top_k, 1, "output top k results");

// Join the dirctory with file name
// @Input	file_dir & file_name
// @Return	file_path
string JoinPath(const string& file_dir, const string& file_name) {
  filesystem::path fs_file_dir = filesystem::path(file_dir);
  filesystem::path fs_file_name = filesystem::path(file_name);
  fs_file_dir /= fs_file_name;
  return fs_file_dir.string();
}

// Split the ground truth label with the image name
// @Input	pair_list
// @Return	image_list & gt_label_list
void SplitImageWithLabel(
    const string& dataset_dir,
    const vector<string>& pair_list,
    vector<string>& image_list,
    vector<string>& gt_label_list) {
  for(auto& pair : pair_list) {
    vector<string> fields;
    split( fields, pair, is_any_of(" "));
    image_list.emplace_back(JoinPath(dataset_dir, fields[0]));
    gt_label_list.emplace_back(fields[1]);
  }
}

vector<vector<string>> SplitImageList(
    const vector<string>& image_list,
    int batch_size) {
  int enlist_num = 0;
  int total_num = image_list.size();
  vector<vector<string>> image_slices;
  while (enlist_num != total_num) {
    if ((total_num - enlist_num) < batch_size) {
      image_slices.push_back(vector<string>(image_list.begin() + enlist_num, image_list.end()));
      enlist_num = total_num;
      break;
    }
    auto local_begin = image_list.begin() + enlist_num;
    image_slices.push_back(vector<string>(local_begin, local_begin + batch_size));
    enlist_num += batch_size;
  }
  return image_slices;
}

int main(int argc, char** argv) {
  /******** Parse flags ********/
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  /******** Init classifier ********/
  unique_ptr<Classifier> classifier;
  classifier.reset(new Classifier(
      FLAGS_engine_file, FLAGS_output_blob_file,
      FLAGS_mean_file, FLAGS_label_file,
      FLAGS_stdvar, FLAGS_base_size, FLAGS_top_k));
  int batch_size = classifier->GetBatchSize();
  std::cout << "batch size: " << batch_size << std::endl;
  /******** Read ground truth file ********/
  vector<string> image_label_pair_list;
  image_label_pair_list = ReadFileByLine(FLAGS_gt_text);
  // test images' path list
  vector<string> image_list;
  // test images' ground truth label list
  vector<string> gt_label_list;
  SplitImageWithLabel(FLAGS_dataset_dir, image_label_pair_list, image_list, gt_label_list);
  // print image_list & gt_labels_list
  assert(image_list.size() == gt_label_list.size());
  int total_num = image_list.size();
  if (FLAGS_verbose) {
    for (int i = 0; i < total_num; i++) {
      cout <<"image path: " << image_list[i] << "\t"
          << "ground truth label: " << gt_label_list[i] << endl;
    }
  }

  /******** Process images in a mini-batch ********/
  // split images into slices
  auto image_slices = SplitImageList(image_list, batch_size);
  // print splitted image slices
  if (FLAGS_verbose) {
    for (int i = 0; i < image_slices.size(); i++) {
      cout << "slice " << i << ":" << endl;
      for (int j = 0; j < image_slices[i].size(); j++) {
        cout << image_slices[i][j] << endl;
      }
    }
  }
  vector<vector<string>> result_list;
  //process images in batch
  uint64_t infer_time = 0;
  for (int i = 0; i < image_slices.size(); i++) {
    cout << "processing " << i <<"th slice" << endl;
    // read images in batch
    vector<cv::Mat> mats;
    for (int j = 0; j < image_slices[i].size(); j++) {
      mats.push_back(cv::imread(image_slices[i][j]));
    }
    // do inference
    auto tic = GetTimeMilli();
    auto results = classifier->Classify(mats);
    auto toc = GetTimeMilli();
    infer_time += (toc - tic);
    for (int j = 0; j < results.size(); j++) {
      auto& result = results[j];
      vector<string> predicts;
      for (int k = 0; k < result.size(); k++) {
        predicts.emplace_back(result[k].first);
      }
      // append test result into result list
      result_list.emplace_back(predicts);
    }
  }
  assert(total_num == result_list.size());
  cout << "Infer Time Cost: " << infer_time << "ms" << endl;
  cout << "fps: " << float(total_num) * 1000.0 / infer_time << endl;
  /******** Compare the result & ground truth list ********/
  int correct_num = 0;
  vector<string> output_list;
  for (int i = 0; i < total_num; i++) {
    auto& result = result_list[i];
    if (find(result.begin(), result.end(), gt_label_list[i]) != result.end()) {
      correct_num++;
    }
    if (FLAGS_output_path != "") {
      string outputline = "Top " + std::to_string(FLAGS_top_k) + ":";
      for (int j = 0; j < result.size(); j++) {
        outputline += result[j];
        outputline += "\t";
      }
      outputline += ("GT: " + gt_label_list[i]);
      output_list.emplace_back(outputline);
    }
  }
  float accuracy = float(correct_num) / float(total_num);
  cout << "Total num: " << total_num << endl;
  cout << "Correct num: " << correct_num << endl;
  cout << "Accuracy: " << accuracy << endl;
  if (FLAGS_output_path != "") {
    WriteFileByLine(FLAGS_output_path, output_list);
  }
  return 0;
}

ext: cpp

max_stars_repo_name: Adonishl/TensorRTClassifier

max_issues_repo_name: liushuchun/TensorRTClassifier

max_forks_repo_name: liushuchun/TensorRTClassifier


text:
/**
 * @file   gtest.hpp
 * @author Paul Furgale <paul.furgale@gmail.com>
 * @date   Mon Dec 12 11:54:20 2011
 *
 * @brief  Code to simplify Eigen integration into GTest. Pretty basic but the error messages are good.
 *
 *
 */
#ifndef SM_EIGEN_GTEST_HPP
#define SM_EIGEN_GTEST_HPP

#include <gtest/gtest.h>
#include <sm/source_file_pos.hpp>
#include <Eigen/Core>

namespace sm { namespace eigen {

    template<int N>
    Eigen::Matrix<double,N,N> randomCovariance()
    {
      Eigen::Matrix<double,N,N> U;
      U.setRandom();
      return U.transpose() * U + 5.0 * Eigen::Matrix<double,N,N>::Identity();
    }

    inline Eigen::MatrixXd randomCovarianceXd(int N)
    {
        Eigen::MatrixXd U(N,N);
        U.setRandom();
        return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N);
    }

    template<typename M1, typename M2>
    void assertEqual(const M1 & A, const M2 & B, sm::source_file_pos const & sfp, std::string const & message = "")
    {
        ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();
      ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();

      for(int r = 0; r < A.rows(); r++)
		{
		  for(int c = 0; c < A.cols(); c++)
			{
			  ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
                                       << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
			}
		}
    }

    template<typename M1, typename M2, typename T>
    void assertNear(const M1 & A, const M2 & B, T tolerance, sm::source_file_pos const & sfp, std::string const & message = "")
    {
      // Note: If these assertions fail, they only abort this subroutine.
      // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
      // \todo better handling of this
        ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();
      ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();

      for(int r = 0; r < A.rows(); r++)
		{
		  for(int c = 0; c < A.cols(); c++)
			{
			  ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
												   << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
			}
		}
    }

    template<typename M1, typename M2, typename T>
    void expectNear(const M1 & A, const M2 & B, T tolerance, sm::source_file_pos const & sfp, std::string const & message = "")
    {
      EXPECT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();
      EXPECT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString();

      for(int r = 0; r < A.rows(); r++)
        {
          for(int c = 0; c < A.cols(); c++)
            {
              EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString()
												   << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
            }
        }
    }

    template<typename M1>
    void assertFinite(const M1 & A, sm::source_file_pos const & sfp, std::string const & message = "")
    {
      for(int r = 0; r < A.rows(); r++)
		{
		  for(int c = 0; c < A.cols(); c++)
			{
			  ASSERT_TRUE(std::isfinite(A(r,c))) << sfp.toString() << std::endl << message << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl;
			}
		}
    }

    inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL)
    {
	  // \todo: does anyone have a better idea?
      double fa = fabs(a);
      double fb = fabs(b);
      if( (fa < 1e-15 && fb < 1e-15) ||  // Both zero.
		  (fa == 0.0  && fb < 1e-6)  ||  // One exactly zero and the other small
		  (fb == 0.0  && fa < 1e-6) )    // ditto
		return true;

      double diff = fabs(a - b)/std::max(fa,fb);
      if(diff > percentTolerance * 1e-2)
		{
		  if(percentError)
			*percentError = diff * 100.0;
		  return false;
		}
      return true;
    }

#define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)				\
    ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
    ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols())  << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
    for(int r = 0; r < (A).rows(); r++)									\
      {																	\
		for(int c = 0; c < (A).cols(); c++)								\
		  {																\
			double percentError = 0.0;									\
			ASSERT_TRUE(sm::eigen::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
			  << MSG << "\nComparing:\n"								\
			  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
			  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
			  << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
			  << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
		  }																\
      }

  }} // namespace sm::eigen

#endif /* SM_EIGEN_GTEST_HPP */

ext: hpp

max_stars_repo_name: PushyamiKaveti/kalibr

max_issues_repo_name: PushyamiKaveti/kalibr

max_forks_repo_name: PushyamiKaveti/kalibr


text:
// Copyright (c) Prevail Verifier contributors.
// SPDX-License-Identifier: Apache-2.0
/*******************************************************************************
 *
 * Difference Bound Matrix domain based on the paper "Exploiting
 * Sparsity in Difference-Bound Matrices" by Gange, Navas, Schachte,
 * Sondergaard, and Stuckey published in SAS'16.

 * A re-engineered implementation of the Difference Bound Matrix
 * domain, which maintains bounds and relations separately.
 *
 * Closure operations based on the paper "Fast and Flexible Difference
 * Constraint Propagation for DPLL(T)" by Cotton and Maler.
 *
 * Author: Graeme Gange (gkgange@unimelb.edu.au)
 *
 * Contributors: Jorge A. Navas (jorge.navas@sri.com)
 ******************************************************************************/

#pragma once

#include <optional>
#include <type_traits>
#include <unordered_set>
#include <utility>

#include <boost/container/flat_map.hpp>

#include "crab/interval.hpp"
#include "crab/linear_constraint.hpp"
#include "crab/thresholds.hpp"
#include "crab/variable.hpp"
#include "crab_utils/adapt_sgraph.hpp"
#include "crab_utils/bignums.hpp"
#include "crab_utils/debug.hpp"
#include "crab_utils/graph_ops.hpp"
#include "crab_utils/safeint.hpp"
#include "crab_utils/stats.hpp"

#include "string_constraints.hpp"

// These constants are mostly used in ebpf_domain.cpp, but some uses
// in split_dbm.cpp and array_domain.cpp require them to be declared here.
// The exact numbers are taken advantage of in ebpf_domain_t
enum type_encoding_t {
    T_UNINIT = -7,
    T_MAP_PROGRAMS = -6,
    T_MAP = -5,
    T_NUM = -4,
    T_CTX = -3,
    T_STACK = -2,
    T_PACKET = -1,
    T_SHARED = 0
};

namespace crab {

enum class arith_binop_t { ADD, SUB, MUL, SDIV, UDIV, SREM, UREM };
enum class bitwise_binop_t { AND, OR, XOR, SHL, LSHR, ASHR };
using binop_t = std::variant<arith_binop_t, bitwise_binop_t>;

namespace domains {

/** DBM weights (Weight) can be represented using one of the following
 * types:
 *
 * 1) basic integer type: e.g., long
 * 2) safei64
 * 3) z_number
 *
 * 1) is the fastest but things can go wrong if some DBM
 * operation overflows. 2) is slower than 1) but it checks for
 * overflow before any DBM operation. 3) is the slowest and it
 * represents weights using unbounded mathematical integers so
 * overflow is not a concern but it might not be what you need
 * when reasoning about programs with wraparound semantics.
 **/

struct SafeInt64DefaultParams {
    using Weight = safe_i64;
    using graph_t = AdaptGraph;
};

/**
 * Helper to translate from Number to DBM Weight (graph weights).  Number
 * used to be the template parameter of the DBM-based abstract domain to
 * represent a number. Number might not fit into Weight type.
 **/
inline SafeInt64DefaultParams::Weight convert_NtoW(const z_number& n, bool& overflow) {
    overflow = false;
    if (!n.fits_sint64()) {
        overflow = true;
        return 0;
    }
    return SafeInt64DefaultParams::Weight(n);
}

class SplitDBM final {
  private:
    using variable_vector_t = std::vector<variable_t>;

    using Params = SafeInt64DefaultParams;
    using Weight = typename Params::Weight;
    using graph_t = typename Params::graph_t;
    using vert_id = typename graph_t::vert_id;
    using vert_map_t = boost::container::flat_map<variable_t, vert_id>;
    using vmap_elt_t = typename vert_map_t::value_type;
    using rev_map_t = std::vector<std::optional<variable_t>>;
    using GrOps = GraphOps<graph_t>;
    using GrPerm = GraphPerm<graph_t>;
    using edge_vector = typename GrOps::edge_vector;
    // < <x, y>, k> == x - y <= k.
    using diffcst_t = std::pair<std::pair<variable_t, variable_t>, Weight>;
    using vert_set_t = std::unordered_set<vert_id>;

  private:
    //================
    // Domain data
    //================
    // GKG: ranges are now maintained in the graph
    vert_map_t vert_map; // Mapping from variables to vertices
    rev_map_t rev_map;
    graph_t g;                 // The underlying relation graph
    std::vector<Weight> potential; // Stored potential for the vertex
    vert_set_t unstable;
    bool _is_bottom;

    vert_id get_vert(variable_t v);

    class vert_set_wrap_t {
      public:
        explicit vert_set_wrap_t(const vert_set_t& _vs) : vs(_vs) {}

        bool operator[](vert_id v) const { return vs.find(v) != vs.end(); }
        const vert_set_t& vs;
    };

    // Evaluate the potential value of a variable.
    Weight pot_value(variable_t v) const {
        auto it = vert_map.find(v);
        if (it != vert_map.end())
            return potential[(*it).second];
        return ((Weight)0);
    }

    // Evaluate an expression under the chosen potentials
    Weight eval_expression(const linear_expression_t& e, bool overflow) const {
        Weight res(convert_NtoW(e.constant_term(), overflow));
        if (overflow) {
            return Weight(0);
        }

        for (const auto& [variable, coefficient] : e.variable_terms()) {
            Weight coef = convert_NtoW(coefficient, overflow);
            if (overflow) {
                return Weight(0);
            }
            res += (pot_value(variable) - potential[0]) * coef;
        }
        return res;
    }

    interval_t compute_residual(const linear_expression_t& e, variable_t pivot) const {
        interval_t residual(-e.constant_term());
        for (const auto& [variable, coefficient] : e.variable_terms()) {
            if (variable != pivot) {
                residual = residual - (interval_t(coefficient) * this->operator[](variable));
            }
        }
        return residual;
    }

    /**
     *  Turn an assignment into a set of difference constraints.
     *
     *  Given v := a*x + b*y + k, where a,b >= 0, we generate the
     *  difference constraints:
     *
     *  if extract_upper_bounds
     *     v - x <= ub((a-1)*x + b*y + k)
     *     v - y <= ub(a*x + (b-1)*y + k)
     *  else
     *     x - v <= lb((a-1)*x + b*y + k)
     *     y - v <= lb(a*x + (b-1)*y + k)
     **/
    void diffcsts_of_assign(variable_t x, const linear_expression_t& exp,
                            /* if true then process the upper
                               bounds, else the lower bounds */
                            bool extract_upper_bounds,
                            /* foreach {v, k} \in diff_csts we have
                               the difference constraint v - k <= k */
                            std::vector<std::pair<variable_t, Weight>>& diff_csts) const;

    // Turn an assignment into a set of difference constraints.
    void diffcsts_of_assign(variable_t x, const linear_expression_t& exp, std::vector<std::pair<variable_t, Weight>>& lb,
                            std::vector<std::pair<variable_t, Weight>>& ub) {
        diffcsts_of_assign(x, exp, true, ub);
        diffcsts_of_assign(x, exp, false, lb);
    }

    /**
     * Turn a linear inequality into a set of difference
     * constraints.
     **/
    void diffcsts_of_lin_leq(const linear_expression_t& exp,
                             /* difference contraints */
                             std::vector<diffcst_t>& csts,
                             /* x >= lb for each {x,lb} in lbs */
                             std::vector<std::pair<variable_t, Weight>>& lbs,
                             /* x <= ub for each {x,ub} in ubs */
                             std::vector<std::pair<variable_t, Weight>>& ubs) const;

    bool add_linear_leq(const linear_expression_t& exp);

    // x != n
    void add_univar_disequation(variable_t x, const number_t& n);

    void add_disequation(const linear_expression_t& e) {
        // XXX: similar precision as the interval domain
        for (const auto& [variable, coefficient] : e.variable_terms()) {
            interval_t i = compute_residual(e, variable) / interval_t(coefficient);
            if (auto k = i.singleton()) {
                add_univar_disequation(variable, *k);
            }
        }
        normalize();
    }

    interval_t get_interval(variable_t x) const { return get_interval(vert_map, g, x); }

    static interval_t get_interval(const vert_map_t& m, const graph_t& r, variable_t x) {
        auto it = m.find(x);
        if (it == m.end()) {
            return interval_t::top();
        }
        vert_id v = (*it).second;
        interval_t x_out = interval_t(r.elem(v, 0) ? -number_t(r.edge_val(v, 0)) : bound_t::minus_infinity(),
                                      r.elem(0, v) ? number_t(r.edge_val(0, v)) : bound_t::plus_infinity());
        return x_out;
    }

    // Restore potential after an edge addition
    bool repair_potential(vert_id src, vert_id dest) { return GrOps::repair_potential(g, potential, src, dest); }

    // Restore closure after a single edge addition
    void close_over_edge(vert_id ii, vert_id jj);

  public:
    explicit SplitDBM(bool is_bottom = false) : _is_bottom(is_bottom) {
        g.growTo(1); // Allocate the zero vector
        potential.emplace_back(0);
        rev_map.push_back(std::nullopt);
    }

    // FIXME: Rewrite to avoid copying if o is _|_
    SplitDBM(vert_map_t&& _vert_map, rev_map_t&& _rev_map, graph_t&& _g, std::vector<Weight>&& _potential,
             vert_set_t&& _unstable)
        : vert_map(std::move(_vert_map)), rev_map(std::move(_rev_map)), g(std::move(_g)),
          potential(std::move(_potential)), unstable(std::move(_unstable)), _is_bottom(false) {

        CrabStats::count("SplitDBM.count.copy");
        ScopedCrabStats __st__("SplitDBM.copy");

        CRAB_LOG("zones-split-size", auto p = size();
                 std::cout << "#nodes = " << p.first << " #edges=" << p.second << "\n";);

        assert(g.size() > 0);
        normalize();
    }

    SplitDBM(const SplitDBM& o) = default;
    SplitDBM(SplitDBM&& o) = default;

    SplitDBM& operator=(const SplitDBM& o) = default;
    SplitDBM& operator=(SplitDBM&& o) = default;

    void set_to_top() {
        this->~SplitDBM();
        new (this) SplitDBM(false);
    }

    void set_to_bottom() {
        this->~SplitDBM();
        new (this) SplitDBM(true);
    }

    bool is_bottom() const { return _is_bottom; }

    static SplitDBM top() { return SplitDBM(false); }

    static SplitDBM bottom() { return SplitDBM(true); }

    bool is_top() const {
        if (_is_bottom)
            return false;
        return g.is_empty();
    }

    bool operator<=(const SplitDBM& o) const;

    // FIXME: can be done more efficient
    void operator|=(const SplitDBM& o) { *this = *this | o; }
    void operator|=(SplitDBM&& o) {
        if (is_bottom()) {
            std::swap(*this, o);
        } else {
            *this = *this | o;
        }
    }

    SplitDBM operator|(const SplitDBM& o) const&;

    SplitDBM operator|(SplitDBM&& o) && {
        if (is_bottom() || o.is_top())
            return std::move(o);
        if (is_top() || o.is_bottom())
            return std::move(*this);
        return ((const SplitDBM&)*this) | (const SplitDBM&)o;
    }

    SplitDBM operator|(const SplitDBM& o) && {
        if (is_top() || o.is_bottom())
            return std::move(*this);
        return ((const SplitDBM&)*this) | o;
    }

    SplitDBM operator|(SplitDBM&& o) const& {
        if (is_bottom() || o.is_top())
            return std::move(o);
        return (*this) | (const SplitDBM&)o;
    }

    SplitDBM widen(const SplitDBM& o) const;

    SplitDBM widening_thresholds(const SplitDBM& o, const iterators::thresholds_t& ts) const {
        // TODO: use thresholds
        return this->widen(o);
    }

    SplitDBM operator&(const SplitDBM& o) const;

    SplitDBM narrow(const SplitDBM& o) const;

    void normalize();

    void operator-=(variable_t v);

    void assign(variable_t x, const linear_expression_t& e);

    void assign(std::optional<variable_t> x, const linear_expression_t& e) {
        if (x) {
            assign(*x, e);
        }
    }
    void assign(variable_t x, signed long long int n) { assign(x, linear_expression_t(n)); }

    void assign(variable_t x, variable_t v) {
        assign(x, linear_expression_t{v});
    }
    void assign(variable_t x, const std::optional<linear_expression_t>& e) {
        if (e) {
            assign(x, *e);
        } else {
            *this -= x;
        }
    };

    void apply(arith_binop_t op, variable_t x, variable_t y, variable_t z);

    void apply(arith_binop_t op, variable_t x, variable_t y, const number_t& k);

    // bitwise_operators_api
    void apply(bitwise_binop_t op, variable_t x, variable_t y, variable_t z);

    void apply(bitwise_binop_t op, variable_t x, variable_t y, const number_t& k);

    void apply(binop_t op, variable_t x, variable_t y, const number_t& z) {
        std::visit([&](auto top) { apply(top, x, y, z); }, op);
    }

    void apply(binop_t op, variable_t x, variable_t y, variable_t z) {
        std::visit([&](auto top) { apply(top, x, y, z); }, op);
    }

    void operator+=(const linear_constraint_t& cst);

    SplitDBM when(const linear_constraint_t& cst) const {
        SplitDBM res(*this);
        res += cst;
        return res;
    }

    interval_t eval_interval(const linear_expression_t& e) const {
        interval_t r{e.constant_term()};
        for (const auto& [variable, coefficient] : e.variable_terms())
            r += coefficient * operator[](variable);
        return r;
    }

    interval_t operator[](variable_t x) const {
        if (is_bottom()) {
            return interval_t::bottom();
        } else {
            return get_interval(vert_map, g, x);
        }
    }

    void set(variable_t x, const interval_t& intv);

    void forget(const variable_vector_t& variables);

    // -- begin array_sgraph_domain_helper_traits

    // -- end array_sgraph_domain_helper_traits

    // return number of vertices and edges
    std::pair<std::size_t, std::size_t> size() const { return {g.size(), g.num_edges()}; }

  private:
    bool entail_aux(const linear_constraint_t& cst) const {
        SplitDBM dom(*this); // copy is necessary
        dom += cst.negate();
        return dom.is_bottom();
    }

    bool intersect_aux(const linear_constraint_t& cst) const {
        SplitDBM dom(*this); // copy is necessary
        dom += cst;
        return !dom.is_bottom();
    }

  public:
    /*
       Public API

       bool entail(const linear_constraint_t&);

       bool intersect(const linear_constraint_t&);
     */

    // Return true if inv intersects with cst.
    bool intersect(const linear_constraint_t& cst) const {
        if (is_bottom() || cst.is_contradiction())
            return false;
        if (is_top() || cst.is_tautology())
            return true;
        return intersect_aux(cst);
    }

    // Return true if entails rhs.
    bool entail(const linear_constraint_t& rhs) const {
        if (is_bottom())
            return true;
        if (rhs.is_tautology())
            return true;
        if (rhs.is_contradiction())
            return false;
        interval_t interval = eval_interval(rhs.expression());
        switch (rhs.kind()) {
        case constraint_kind_t::EQUALS_ZERO:
            if (interval.singleton() == std::optional<number_t>(number_t(0)))
                return true;
            break;
        case constraint_kind_t::LESS_THAN_OR_EQUALS_ZERO:
            if (interval.ub() <= number_t(0))
                return true;
            break;
        case constraint_kind_t::LESS_THAN_ZERO:
            if (interval.ub() < number_t(0))
                return true;
            break;
        case constraint_kind_t::NOT_ZERO:
            if (interval.ub() < number_t(0) || interval.lb() > number_t(0))
                return true;
            break;
        }
        if (rhs.kind() == constraint_kind_t::EQUALS_ZERO) {
            // try to convert the equality into inequalities so when it's
            // negated we do not have disequalities.
            return entail_aux(linear_constraint_t(rhs.expression(), constraint_kind_t::LESS_THAN_OR_EQUALS_ZERO)) &&
                   entail_aux(linear_constraint_t(rhs.expression() * number_t(-1),
                                                  constraint_kind_t::LESS_THAN_OR_EQUALS_ZERO));
        } else {
            return entail_aux(rhs);
        }

        // Note: we cannot convert rhs into SplitDBM and then use the <=
        //       operator. The problem is that we cannot know for sure
        //       whether SplitDBM can represent precisely rhs. It is not
        //       enough to do something like
        //
        //       SplitDBM dom = rhs;
        //       if (dom.is_top()) { ... }
    }

    friend std::ostream& operator<<(std::ostream& o, const SplitDBM& dom);
    string_invariant to_set() const;
}; // class SplitDBM

} // namespace domains
} // namespace crab

max_stars_repo_name: dthaler/ebpf-verifier

max_issues_repo_name: dthaler/ebpf-verifier

max_forks_repo_name: dthaler/ebpf-verifier


text:
#include <cppcon/test/async_acceptor.hpp>

#include <cstddef>
#include <optional>
#include <stdexcept>
#include <system_error>
#include <utility>
#include <boost/asio/ts/io_context.hpp>
#include <cppcon/test/async_write_stream.hpp>

#include <catch2/catch.hpp>

namespace cppcon {
namespace test {
namespace tests {
namespace {

TEST_CASE("Nullary complete",
          "[async_acceptor]")
{
  std::error_code ec;
  boost::asio::io_context a;
  boost::asio::io_context b;
  using stream_type = async_write_stream<boost::asio::io_context::executor_type>;
  std::optional<stream_type> stream;
  async_acceptor<boost::asio::io_context::executor_type,
                 stream_type> acc(a.get_executor());
  auto f = [&](auto e,
               auto s)
           {
             if (stream) {
               throw std::logic_error("Already invoked");
             }
             ec = e;
             stream.emplace(std::move(s));
           };
  acc.async_accept(b.get_executor(),
                   f);
  CHECK_FALSE(stream);
  REQUIRE(acc.pending());
  CHECK(acc.get_stream_executor() == b.get_executor());
  auto handlers = a.poll();
  CHECK_FALSE(handlers);
  CHECK_FALSE(a.stopped());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
  b.restart();
  acc.complete();
  CHECK_FALSE(acc.pending());
  CHECK_FALSE(stream);
  handlers = a.poll();
  CHECK(handlers == 1);
  CHECK(a.stopped());
  REQUIRE(stream);
  CHECK_FALSE(ec);
  CHECK(stream->get_executor() == b.get_executor());
  CHECK_FALSE(stream->remaining());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
}

TEST_CASE("Complete w/args (success)",
          "[async_acceptor]")
{
  std::byte buffer[1024];
  std::error_code ec;
  boost::asio::io_context a;
  boost::asio::io_context b;
  using stream_type = async_write_stream<boost::asio::io_context::executor_type>;
  std::optional<stream_type> stream;
  async_acceptor<boost::asio::io_context::executor_type,
                 stream_type> acc(a.get_executor());
  auto f = [&](auto e,
               auto s)
           {
             if (stream) {
               throw std::logic_error("Already invoked");
             }
             ec = e;
             stream.emplace(std::move(s));
           };
  acc.async_accept(b.get_executor(),
                   f);
  CHECK_FALSE(stream);
  REQUIRE(acc.pending());
  CHECK(acc.get_stream_executor() == b.get_executor());
  auto handlers = a.poll();
  CHECK_FALSE(handlers);
  CHECK_FALSE(a.stopped());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
  b.restart();
  acc.complete(std::error_code(),
               buffer,
               sizeof(buffer));
  CHECK_FALSE(acc.pending());
  CHECK_FALSE(stream);
  handlers = a.poll();
  CHECK(handlers == 1);
  CHECK(a.stopped());
  REQUIRE(stream);
  CHECK_FALSE(ec);
  CHECK(stream->get_executor() == b.get_executor());
  CHECK(stream->remaining() == sizeof(buffer));
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
}

TEST_CASE("Complete w/args (failure)",
          "[async_acceptor]")
{
  std::byte buffer[1024];
  std::error_code ec;
  boost::asio::io_context a;
  boost::asio::io_context b;
  using stream_type = async_write_stream<boost::asio::io_context::executor_type>;
  std::optional<stream_type> stream;
  async_acceptor<boost::asio::io_context::executor_type,
                 stream_type> acc(a.get_executor());
  auto f = [&](auto e,
               auto s)
           {
             if (stream) {
               throw std::logic_error("Already invoked");
             }
             ec = e;
             stream.emplace(std::move(s));
           };
  acc.async_accept(b.get_executor(),
                   f);
  CHECK_FALSE(stream);
  REQUIRE(acc.pending());
  CHECK(acc.get_stream_executor() == b.get_executor());
  auto handlers = a.poll();
  CHECK_FALSE(handlers);
  CHECK_FALSE(a.stopped());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
  b.restart();
  acc.complete(make_error_code(std::errc::connection_aborted),
               buffer,
               sizeof(buffer));
  CHECK_FALSE(acc.pending());
  CHECK_FALSE(stream);
  handlers = a.poll();
  CHECK(handlers == 1);
  CHECK(a.stopped());
  REQUIRE(stream);
  CHECK(ec);
  CHECK(ec == make_error_code(std::errc::connection_aborted));
  CHECK(stream->get_executor() == b.get_executor());
  CHECK(stream->remaining() == sizeof(buffer));
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
}

TEST_CASE("Cancel",
          "[async_acceptor]")
{
  std::byte buffer[1024];
  std::error_code ec;
  boost::asio::io_context a;
  boost::asio::io_context b;
  using stream_type = async_write_stream<boost::asio::io_context::executor_type>;
  std::optional<stream_type> stream;
  async_acceptor<boost::asio::io_context::executor_type,
                 stream_type> acc(a.get_executor());
  auto f = [&](auto e,
               auto s)
           {
             if (stream) {
               throw std::logic_error("Already invoked");
             }
             ec = e;
             stream.emplace(std::move(s));
           };
  acc.async_accept(b.get_executor(),
                   f);
  CHECK_FALSE(stream);
  REQUIRE(acc.pending());
  CHECK(acc.get_stream_executor() == b.get_executor());
  auto handlers = a.poll();
  CHECK_FALSE(handlers);
  CHECK_FALSE(a.stopped());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
  b.restart();
  acc.cancel();
  CHECK_FALSE(acc.pending());
  CHECK_FALSE(stream);
  handlers = a.poll();
  CHECK(handlers == 1);
  CHECK(a.stopped());
  REQUIRE(stream);
  CHECK(ec);
  CHECK(ec == make_error_code(std::errc::operation_canceled));
  CHECK(stream->get_executor() == b.get_executor());
  CHECK_FALSE(stream->remaining());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
}

TEST_CASE("Accept on context",
          "[async_acceptor]")
{
  std::error_code ec;
  boost::asio::io_context a;
  boost::asio::io_context b;
  using stream_type = async_write_stream<boost::asio::io_context::executor_type>;
  std::optional<stream_type> stream;
  async_acceptor<boost::asio::io_context::executor_type,
                 stream_type> acc(a.get_executor());
  auto f = [&](auto e,
               auto s)
           {
             if (stream) {
               throw std::logic_error("Already invoked");
             }
             ec = e;
             stream.emplace(std::move(s));
           };
  acc.async_accept(b,
                   f);
  CHECK_FALSE(stream);
  REQUIRE(acc.pending());
  CHECK(acc.get_stream_executor() == b.get_executor());
  auto handlers = a.poll();
  CHECK_FALSE(handlers);
  CHECK_FALSE(a.stopped());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
  b.restart();
  acc.complete();
  CHECK_FALSE(acc.pending());
  CHECK_FALSE(stream);
  handlers = a.poll();
  CHECK(handlers == 1);
  CHECK(a.stopped());
  REQUIRE(stream);
  CHECK_FALSE(ec);
  CHECK(stream->get_executor() == b.get_executor());
  CHECK_FALSE(stream->remaining());
  handlers = b.poll();
  CHECK_FALSE(handlers);
  CHECK(b.stopped());
}

}
}
}
}


text:
#include <boost/function.hpp>
#include <functional>
#include <iostream>

struct world
{
  void hello(std::ostream &os)
  {
    os << "Hello, world!\n";
  }
};

int main()
{
  boost::function<void(world*, std::ostream&)> f = &world::hello;
  world w;
  f(&w, std::ref(std::cout));
}

max_stars_repo_name: KwangjoJeong/Boost

max_issues_repo_name: KwangjoJeong/Boost

max_forks_repo_name: KwangjoJeong/Boost


text:
#ifndef YOU_BOT_BASE_KINEMATICS_TEST_H
#define YOU_BOT_BASE_KINEMATICS_TEST_H

#include <cppunit/TestCase.h>
#include <cppunit/extensions/HelperMacros.h>

#include <iostream>
#include <vector>
#include <signal.h>
#include <boost/ptr_container/ptr_vector.hpp>
#include "youbot_driver/youbot/YouBotBase.hpp"
#include "youbot_driver/youbot/YouBotManipulator.hpp"
#include "youbot_driver/youbot/DataTrace.hpp"

using namespace youbot;

///////////////////////////////////////////////////////////////////////////////
/// A unit test for the base kinematics
///////////////////////////////////////////////////////////////////////////////
class YouBotBaseKinematicsTest : public CppUnit::TestFixture {
	CPPUNIT_TEST_SUITE(YouBotBaseKinematicsTest);
	CPPUNIT_TEST(youBotBaseKinematicsTest);
	CPPUNIT_TEST_SUITE_END();

public:
	YouBotBaseKinematicsTest();
	virtual ~YouBotBaseKinematicsTest();

	void setUp();
	void tearDown();

	void youBotBaseKinematicsTest();

private:
	unsigned int overallTime;
	unsigned int startTime;
	unsigned int updateCycle;
};

#endif //YOU_BOT_BASE_KINEMATICS_TEST_H

max_stars_repo_name: MrJaeqx/ESA--WORK

max_issues_repo_name: MrJaeqx/ESA--WORK

max_forks_repo_name: FontysAtWork/ESA-PROJ


text:
// Copyright (C) 2011-2020 Internet Systems Consortium, Inc. ("ISC")
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

/// @file
/// Access to interface information on Linux is via netlink, a socket-based
/// method for transferring information between the kernel and user processes.
///
/// For detailed information about netlink interface, please refer to
/// http://en.wikipedia.org/wiki/Netlink and RFC3549.  Comments in the
/// detectIfaces() method (towards the end of this file) provide an overview
/// on how the netlink interface is used here.
///
/// Note that this interface is very robust and allows many operations:
/// add/get/set/delete links, addresses, routes, queuing, manipulation of
/// traffic classes, manipulation of neighbourhood tables and even the ability
/// to do something with address labels. Getting a list of interfaces with
/// addresses configured on it is just a small subset of all possible actions.

#include <config.h>

#if defined(OS_LINUX)

#include <asiolink/io_address.h>
#include <dhcp/iface_mgr.h>
#include <dhcp/iface_mgr_error_handler.h>
#include <dhcp/pkt_filter_inet.h>
#include <dhcp/pkt_filter_lpf.h>
#include <exceptions/exceptions.h>
#include <util/io/sockaddr_util.h>

#include <boost/array.hpp>
#include <boost/static_assert.hpp>

#include <fcntl.h>
#include <stdint.h>
#include <net/if.h>
#include <linux/rtnetlink.h>

using namespace std;
using namespace isc;
using namespace isc::asiolink;
using namespace isc::dhcp;
using namespace isc::util::io::internal;

BOOST_STATIC_ASSERT(IFLA_MAX>=IFA_MAX);

namespace {

/// @brief This class offers utility methods for netlink connection.
///
/// See IfaceMgr::detectIfaces() (Linux implementation, towards the end of this
/// file) for example usage.
class Netlink
{
public:

/// @brief Holds pointers to netlink messages.
///
/// netlink (a Linux interface for getting information about network
/// interfaces) uses memory aliasing. Linux kernel returns a memory
/// blob that should be interpreted as series of nlmessages. There
/// are different nlmsg structures defined with varying size. They
/// have one thing common - initial fields are laid out in the same
/// way as nlmsghdr. Therefore different messages can be represented
/// as nlmsghdr with followed variable number of bytes that are
/// message-specific. The only reasonable way to represent this in
/// C++ is to use vector of pointers to nlmsghdr (the common structure).
    typedef vector<nlmsghdr*> NetlinkMessages;

/// @brief Holds pointers to interface or address attributes.
///
/// Note that to get address info, a shorter (IFA_MAX rather than IFLA_MAX)
/// table could be used, but we will use the bigger one anyway to
/// make the code reusable.
///
/// rtattr is a generic structure, similar to sockaddr. It is defined
/// in linux/rtnetlink.h and shown here for documentation purposes only:
///
/// struct rtattr {
///     unsigned short<>rta_len;
///     unsigned short<>rta_type;
/// };
    typedef boost::array<struct rtattr*, IFLA_MAX + 1> RTattribPtrs;

    Netlink() : fd_(-1), seq_(0), dump_(0) {
        memset(&local_, 0, sizeof(struct sockaddr_nl));
        memset(&peer_, 0, sizeof(struct sockaddr_nl));
    }

    ~Netlink() {
        rtnl_close_socket();
    }

    void rtnl_open_socket();
    void rtnl_send_request(int family, int type);
    void rtnl_store_reply(NetlinkMessages& storage, const nlmsghdr* msg);
    void parse_rtattr(RTattribPtrs& table, rtattr* rta, int len);
    void ipaddrs_get(Iface& iface, NetlinkMessages& addr_info);
    void rtnl_process_reply(NetlinkMessages& info);
    void release_list(NetlinkMessages& messages);
    void rtnl_close_socket();

private:
    int fd_;            // Netlink file descriptor
    sockaddr_nl local_; // Local addresses
    sockaddr_nl peer_;  // Remote address
    uint32_t seq_;      // Counter used for generating unique sequence numbers
    uint32_t dump_;     // Number of expected message response
};

/// @brief defines a size of a sent netlink buffer
const static size_t SNDBUF_SIZE = 32768;

/// @brief defines a size of a received netlink buffer
const static size_t RCVBUF_SIZE = 32768;

/// @brief Opens netlink socket and initializes handle structure.
///
/// @throw isc::Unexpected Thrown if socket configuration fails.
void Netlink::rtnl_open_socket() {

    fd_ = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
    if (fd_ < 0) {
        isc_throw(Unexpected, "Failed to create NETLINK socket.");
    }

    if (fcntl(fd_, F_SETFD, FD_CLOEXEC) < 0) {
        isc_throw(Unexpected, "Failed to set close-on-exec in NETLINK socket.");
    }

    if (setsockopt(fd_, SOL_SOCKET, SO_SNDBUF, &SNDBUF_SIZE, sizeof(SNDBUF_SIZE)) < 0) {
        isc_throw(Unexpected, "Failed to set send buffer in NETLINK socket.");
    }

    if (setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &RCVBUF_SIZE, sizeof(RCVBUF_SIZE)) < 0) {
        isc_throw(Unexpected, "Failed to set receive buffer in NETLINK socket.");
    }

    local_.nl_family = AF_NETLINK;
    local_.nl_groups = 0;

    if (::bind(fd_, convertSockAddr(&local_), sizeof(local_)) < 0) {
        isc_throw(Unexpected, "Failed to bind netlink socket.");
    }

    socklen_t addr_len = sizeof(local_);
    if (getsockname(fd_, convertSockAddr(&local_), &addr_len) < 0) {
        isc_throw(Unexpected, "Getsockname for netlink socket failed.");
    }

    // just 2 sanity checks and we are done
    if ( (addr_len != sizeof(local_)) ||
         (local_.nl_family != AF_NETLINK) ) {
        isc_throw(Unexpected, "getsockname() returned unexpected data for netlink socket.");
    }
}

/// @brief Closes netlink communication socket
void Netlink::rtnl_close_socket() {
    if (fd_ != -1) {
        close(fd_);
    }
    fd_ = -1;
}

/// @brief Sends request over NETLINK socket.
///
/// @param family requested information family.
/// @param type request type (RTM_GETLINK or RTM_GETADDR).
void Netlink::rtnl_send_request(int family, int type) {
    struct Req {
        nlmsghdr netlink_header;
        rtgenmsg generic;
    };
    Req req; // we need this type named for offsetof() used in assert
    struct sockaddr_nl nladdr;

    // do a sanity check. Verify that Req structure is aligned properly
    BOOST_STATIC_ASSERT(sizeof(nlmsghdr) == offsetof(Req, generic));

    memset(&nladdr, 0, sizeof(nladdr));
    nladdr.nl_family = AF_NETLINK;

    // According to netlink(7) manpage, mlmsg_seq must be set to a sequence
    // number and is used to track messages. That is just a value that is
    // opaque to kernel, and user-space code is supposed to use it to match
    // incoming responses to sent requests. That is not really useful as we
    // send a single request and get a single response at a time. However, we
    // obey the man page suggestion and just set this to monotonically
    // increasing numbers.
    seq_++;

    // This will be used to finding correct response (responses
    // sent by kernel are supposed to have the same sequence number
    // as the request we sent).
    dump_ = seq_;

    memset(&req, 0, sizeof(req));
    req.netlink_header.nlmsg_len = sizeof(req);
    req.netlink_header.nlmsg_type = type;
    req.netlink_header.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
    req.netlink_header.nlmsg_pid = 0;
    req.netlink_header.nlmsg_seq = seq_;
    req.generic.rtgen_family = family;

    int status =  sendto(fd_, static_cast<void*>(&req), sizeof(req), 0,
                         static_cast<struct sockaddr*>(static_cast<void*>(&nladdr)),
                         sizeof(nladdr));

    if (status<0) {
        isc_throw(Unexpected, "Failed to send " << sizeof(nladdr)
                  << " bytes over netlink socket.");
    }
}

/// @brief Appends nlmsg to a storage.
///
/// This method copies pointed nlmsg to a newly allocated memory
/// and adds it to storage.
///
/// @param storage A vector that holds pointers to netlink messages. The caller
///        is responsible for freeing the pointed-to messages.
/// @param msg A netlink message to be added.
void Netlink::rtnl_store_reply(NetlinkMessages& storage, const struct nlmsghdr *msg)
{
    // we need to make a copy of this message. We really can't allocate
    // nlmsghdr directly as it is only part of the structure. There are
    // many message types with varying lengths and a common header.
    struct nlmsghdr* copy = reinterpret_cast<struct nlmsghdr*>(new char[msg->nlmsg_len]);
    memcpy(copy, msg, msg->nlmsg_len);

    // push_back copies only pointer content, not the pointed-to object.
    storage.push_back(copy);
}

/// @brief Parses rtattr message.
///
/// Some netlink messages represent address information. Such messages
/// are concatenated collection of rtaddr structures. This function
/// iterates over that list and stores pointers to those messages in
/// flat array (table).
///
/// @param table rtattr Messages will be stored here
/// @param rta Pointer to first rtattr object
/// @param len Length (in bytes) of concatenated rtattr list.
void Netlink::parse_rtattr(RTattribPtrs& table, struct rtattr* rta, int len)
{
    std::fill(table.begin(), table.end(), static_cast<struct rtattr*>(NULL));
    // RTA_OK and RTA_NEXT() are macros defined in linux/rtnetlink.h
    // they are used to handle rtattributes. RTA_OK checks if the structure
    // pointed by rta is reasonable and passes all sanity checks.
    // RTA_NEXT() returns pointer to the next rtattr structure that
    // immediately follows pointed rta structure. See aforementioned
    // header for details.
    while (RTA_OK(rta, len)) {
        if (rta->rta_type < table.size()) {
            table[rta->rta_type] = rta;
        }
        rta = RTA_NEXT(rta,len);
    }
    if (len) {
        isc_throw(Unexpected, "Failed to parse RTATTR in netlink message.");
    }
}

/// @brief Parses addr_info and appends appropriate addresses to Iface object.
///
/// Netlink is a fine, but convoluted interface. It returns a concatenated
/// collection of netlink messages. Some of those messages convey information
/// about addresses. Those messages are in fact appropriate header followed
/// by concatenated lists of rtattr structures that define various pieces
/// of address information.
///
/// @param iface interface representation (addresses will be added here)
/// @param addr_info collection of parsed netlink messages
void Netlink::ipaddrs_get(Iface& iface, NetlinkMessages& addr_info) {
    uint8_t addr[V6ADDRESS_LEN];
    RTattribPtrs rta_tb;

    for (NetlinkMessages::const_iterator msg = addr_info.begin();
         msg != addr_info.end(); ++msg) {
        ifaddrmsg* ifa = static_cast<ifaddrmsg*>(NLMSG_DATA(*msg));

        // These are not the addresses you are looking for
        if (ifa->ifa_index != iface.getIndex()) {
            continue;
        }

        if ((ifa->ifa_family == AF_INET6) || (ifa->ifa_family == AF_INET)) {
            std::fill(rta_tb.begin(), rta_tb.end(), static_cast<rtattr*>(NULL));
            parse_rtattr(rta_tb, IFA_RTA(ifa), (*msg)->nlmsg_len - NLMSG_LENGTH(sizeof(*ifa)));
            if (!rta_tb[IFA_LOCAL]) {
                rta_tb[IFA_LOCAL] = rta_tb[IFA_ADDRESS];
            }
            if (!rta_tb[IFA_ADDRESS]) {
                rta_tb[IFA_ADDRESS] = rta_tb[IFA_LOCAL];
            }

            memcpy(addr, RTA_DATA(rta_tb[IFLA_ADDRESS]),
                   ifa->ifa_family==AF_INET?V4ADDRESS_LEN:V6ADDRESS_LEN);
            IOAddress a = IOAddress::fromBytes(ifa->ifa_family, addr);
            iface.addAddress(a);

            /// TODO: Read lifetimes of configured IPv6 addresses
        }
    }
}

/// @brief Processes reply received over netlink socket.
///
/// This method parses the received buffer (a collection of concatenated
/// netlink messages), copies each received message to newly allocated
/// memory and stores pointers to it in the "info" container.
///
/// @param info received netlink messages will be stored here.  It is the
///        caller's responsibility to release the memory associated with the
///        messages by calling the release_list() method.
void Netlink::rtnl_process_reply(NetlinkMessages& info) {
    sockaddr_nl nladdr;
    iovec iov;
    msghdr msg;
    memset(&msg, 0, sizeof(msghdr));
    msg.msg_name = &nladdr;
    msg.msg_namelen = sizeof(nladdr);
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;

    char buf[RCVBUF_SIZE];

    iov.iov_base = buf;
    iov.iov_len = sizeof(buf);
    while (true) {
        int status = recvmsg(fd_, &msg, 0);

        if (status < 0) {
            if (errno == EINTR) {
                continue;
            }
            isc_throw(Unexpected, "Error " << errno
                      << " while processing reply from netlink socket.");
        }

        if (status == 0) {
            isc_throw(Unexpected, "EOF while reading netlink socket.");
        }

        nlmsghdr* header = static_cast<nlmsghdr*>(static_cast<void*>(buf));
        while (NLMSG_OK(header, status)) {

            // Received a message not addressed to our process, or not
            // with a sequence number we are expecting.  Ignore, and
            // look at the next one.
            if (nladdr.nl_pid != 0 ||
                header->nlmsg_pid != local_.nl_pid ||
                header->nlmsg_seq != dump_) {
                header = NLMSG_NEXT(header, status);
                continue;
            }

            if (header->nlmsg_type == NLMSG_DONE) {
                // End of message.
                return;
            }

            if (header->nlmsg_type == NLMSG_ERROR) {
                nlmsgerr* err = static_cast<nlmsgerr*>(NLMSG_DATA(header));
                if (header->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
                    // We are really out of luck here. We can't even say what is
                    // wrong as error message is truncated. D'oh.
                    isc_throw(Unexpected, "Netlink reply read failed.");
                } else {
                    isc_throw(Unexpected, "Netlink reply read error " << -err->error);
                }
                // Never happens we throw before we reach here
                return;
            }

            // store the data
            rtnl_store_reply(info, header);

            header = NLMSG_NEXT(header, status);
        }
        if (msg.msg_flags & MSG_TRUNC) {
            isc_throw(Unexpected, "Message received over netlink truncated.");
        }
        if (status) {
            isc_throw(Unexpected, "Trailing garbage of " << status << " bytes received over netlink.");
        }
    }
}

/// @brief releases nlmsg structure
///
/// @param messages Set of messages to be freed.
void Netlink::release_list(NetlinkMessages& messages) {
    // let's free local copies of stored messages
    for (NetlinkMessages::iterator msg = messages.begin(); msg != messages.end(); ++msg) {
        delete[] (*msg);
    }

    // ang get rid of the message pointers as well
    messages.clear();
}

} // end of anonymous namespace

namespace isc {
namespace dhcp {

/// @brief Detect available interfaces on Linux systems.
///
/// Uses the socket-based netlink protocol to retrieve the list of interfaces
/// from the Linux kernel.
void IfaceMgr::detectIfaces() {
    // Copies of netlink messages about links will be stored here.
    Netlink::NetlinkMessages link_info;

    // Copies of netlink messages about addresses will be stored here.
    Netlink::NetlinkMessages addr_info;

    // Socket descriptors and other rtnl-related parameters.
    Netlink nl;

    // Table with pointers to address attributes.
    Netlink::RTattribPtrs attribs_table;
    std::fill(attribs_table.begin(), attribs_table.end(),
              static_cast<struct rtattr*>(NULL));

    // Open socket
    nl.rtnl_open_socket();

    // Now we have open functional socket, let's use it!
    // Ask for list of network interfaces...
    nl.rtnl_send_request(AF_PACKET, RTM_GETLINK);

    // Get reply and store it in link_info list:
    // response is received as with any other socket - just a series
    // of bytes. They are representing collection of netlink messages
    // concatenated together. rtnl_process_reply will parse this
    // buffer, copy each message to a newly allocated memory and
    // store pointers to it in link_info. This allocated memory will
    // be released later. See release_info(link_info) below.
    nl.rtnl_process_reply(link_info);

    // Now ask for list of addresses (AF_UNSPEC = of any family)
    // Let's repeat, but this time ask for any addresses.
    // That includes IPv4, IPv6 and any other address families that
    // are happen to be supported by this system.
    nl.rtnl_send_request(AF_UNSPEC, RTM_GETADDR);

    // Get reply and store it in addr_info list.
    // Again, we will allocate new memory and store messages in
    // addr_info. It will be released later using release_info(addr_info).
    nl.rtnl_process_reply(addr_info);

    // Now build list with interface names
    for (Netlink::NetlinkMessages::iterator msg = link_info.begin();
         msg != link_info.end(); ++msg) {
        // Required to display information about interface
        struct ifinfomsg* interface_info = static_cast<ifinfomsg*>(NLMSG_DATA(*msg));
        int len = (*msg)->nlmsg_len;
        len -= NLMSG_LENGTH(sizeof(*interface_info));
        nl.parse_rtattr(attribs_table, IFLA_RTA(interface_info), len);

        // valgrind reports *possible* memory leak in the line below, but it is
        // bogus. Nevertheless, the whole interface definition has been split
        // into three separate steps for easier debugging.
        const char* tmp = static_cast<const char*>(RTA_DATA(attribs_table[IFLA_IFNAME]));
        string iface_name(tmp); // <--- bogus valgrind warning here
        // This is guaranteed both by the if_nametoindex() implementation
        // and by kernel dev_new_index() code. In fact 0 is impossible too...
        if (interface_info->ifi_index < 0) {
            isc_throw(OutOfRange, "negative interface index");
        }
        IfacePtr iface(new Iface(iface_name, interface_info->ifi_index));

        iface->setHWType(interface_info->ifi_type);
        iface->setFlags(interface_info->ifi_flags);

        // Does interface have LL_ADDR?
        if (attribs_table[IFLA_ADDRESS]) {
            iface->setMac(static_cast<const uint8_t*>(RTA_DATA(attribs_table[IFLA_ADDRESS])),
                          RTA_PAYLOAD(attribs_table[IFLA_ADDRESS]));
        }
        else {
            // Tunnels can have no LL_ADDR. RTA_PAYLOAD doesn't check it and
            // try to dereference it in this manner
        }

        nl.ipaddrs_get(*iface, addr_info);

        // addInterface can now throw so protect against memory leaks.
        try {
            addInterface(iface);
        } catch (...) {
            nl.release_list(link_info);
            nl.release_list(addr_info);
            throw;
        }
    }

    nl.release_list(link_info);
    nl.release_list(addr_info);
}

/// @brief sets flag_*_ fields.
///
/// This implementation is OS-specific as bits have different meaning
/// on different OSes.
///
/// @param flags flags bitfield read from OS
void Iface::setFlags(uint64_t flags) {
    flags_ = flags;

    flag_loopback_ = flags & IFF_LOOPBACK;
    flag_up_ = flags & IFF_UP;
    flag_running_ = flags & IFF_RUNNING;
    flag_multicast_ = flags & IFF_MULTICAST;
    flag_broadcast_ = flags & IFF_BROADCAST;
}

void
IfaceMgr::setMatchingPacketFilter(const bool direct_response_desired) {
    if (direct_response_desired) {
        setPacketFilter(PktFilterPtr(new PktFilterLPF()));

    } else {
        setPacketFilter(PktFilterPtr(new PktFilterInet()));

    }
}

bool
IfaceMgr::openMulticastSocket(Iface& iface,
                              const isc::asiolink::IOAddress& addr,
                              const uint16_t port,
                              IfaceMgrErrorMsgCallback error_handler) {
    // This variable will hold a descriptor of the socket bound to
    // link-local address. It may be required for us to close this
    // socket if an attempt to open and bind a socket to multicast
    // address fails.
    int sock;
    try {
        sock = openSocket(iface.getName(), addr, port,
                          iface.flag_multicast_);

    } catch (const Exception& ex) {
        IFACEMGR_ERROR(SocketConfigError, error_handler,
                       "Failed to open link-local socket on "
                       " interface " << iface.getName() << ": "
                       << ex.what());
        return (false);

    }

    // In order to receive multicast traffic another socket is opened
    // and bound to the multicast address.

    /// @todo The DHCPv6 requires multicast so we may want to think
    /// whether we want to open the socket on a multicast-incapable
    /// interface or not. For now, we prefer to be liberal and allow
    /// it for some odd use cases which may utilize non-multicast
    /// interfaces. Perhaps a warning should be emitted if the
    /// interface is not a multicast one.
    if (iface.flag_multicast_) {
        try {
            openSocket(iface.getName(),
                       IOAddress(ALL_DHCP_RELAY_AGENTS_AND_SERVERS),
                       port);
        } catch (const Exception& ex) {
            // An attempt to open and bind a socket to multicast address
            // has failed. We have to close the socket we previously
            // bound to link-local address - this is everything or
            // nothing strategy.
            iface.delSocket(sock);
            IFACEMGR_ERROR(SocketConfigError, error_handler,
                           "Failed to open multicast socket on"
                           " interface " << iface.getName()
                           << ", reason: " << ex.what());
            return (false);
        }
    }
    // Both sockets have opened successfully.
    return (true);
}

int
IfaceMgr::openSocket6(Iface& iface, const IOAddress& addr, uint16_t port,
                      const bool join_multicast) {
    // Assuming that packet filter is not NULL, because its modifier checks it.
    SocketInfo info = packet_filter6_->openSocket(iface, addr, port,
                                                  join_multicast);
    iface.addSocket(info);

    return (info.sockfd_);
}

} // end of isc::dhcp namespace
} // end of isc namespace

#endif // if defined(LINUX)

ext: cc

max_stars_repo_name: telekom/dt-kea-netconf

max_issues_repo_name: telekom/dt-kea-netconf

max_forks_repo_name: telekom/dt-kea-netconf