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

// Standard includes
#include <fstream>
#include <string>

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include <votca/xtp/staticsite.h>

namespace votca {
namespace xtp {

Eigen::Matrix3d StaticSite::CalculateCartesianMultipole() const {
  // We are transforming here just quadrupoles
  // const  Eigen::VectorXd& MP = _multipole;
  const Vector9d& MP = _Q;
  Eigen::Matrix3d theta = Eigen::Matrix3d::Zero();
  if (_rank > 1) {
    double sqr3 = std::sqrt(3);
    theta(0, 0) = 0.5 * (-MP(4) + sqr3 * MP(7));     // theta_xx
    theta(1, 1) = 0.5 * (-MP(4) - sqr3 * MP(7));     // theta_yy
    theta(2, 2) = MP(4);                             // theta_zz
    theta(0, 1) = theta(1, 0) = 0.5 * sqr3 * MP(8);  // theta_xy = theta_yx
    theta(0, 2) = theta(2, 0) = 0.5 * sqr3 * MP(5);  // theta_xz = theta_zx
    theta(1, 2) = theta(2, 1) = 0.5 * sqr3 * MP(6);  // theta_yz = theta_zy
  }
  return theta;
}

Eigen::VectorXd StaticSite::CalculateSphericalMultipole(
    const Eigen::Matrix3d& quad_cart) {
  Eigen::VectorXd quadrupole_polar = Eigen::VectorXd::Zero(5);
  const double sqr3 = std::sqrt(3);
  quadrupole_polar(0) = quad_cart(2, 2);
  quadrupole_polar(1) = (2. / sqr3) * quad_cart(0, 2);
  quadrupole_polar(2) = (2. / sqr3) * quad_cart(1, 2);
  quadrupole_polar(3) = (1. / sqr3) * (quad_cart(0, 0) - quad_cart(1, 1));
  quadrupole_polar(4) = (2. / sqr3) * quad_cart(0, 1);
  return quadrupole_polar;
}

void StaticSite::Rotate(const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& refPos) {
  Eigen::Vector3d dir = _pos - refPos;
  dir = R * dir;
  _pos = refPos + dir;  // Rotated Position
  if (_rank > 0) {
    const Eigen::Vector3d temp = R * _Q.segment<3>(1);
    _Q.segment<3>(1) = temp;
  }
  if (_rank > 1) {
    Eigen::Matrix3d cartesianquad = CalculateCartesianMultipole();
    Eigen::Matrix3d rotated = R * cartesianquad * R.transpose();
    _Q.segment<5>(4) = CalculateSphericalMultipole(rotated);
  }
  return;
}

void StaticSite::Translate(const Eigen::VectorXd& shift) {
  _pos += shift;
  return;
}

std::string StaticSite::writepolarization() const {
  tools::Elements e;
  double default_pol = 1;  // default is alway 1A^3
  try {
    default_pol =
        e.getPolarizability(_element) * std::pow(tools::conv::nm2ang, 3);
  } catch (const std::runtime_error&) {
    ;
  }
  return (boost::format("     P %1$+1.7f\n") % default_pol).str();
}

std::string StaticSite::WriteMpsLine(std::string unit) const {
  double conv_pos = 1.;
  if (unit == "angstrom") {
    conv_pos = tools::conv::bohr2ang;
  } else if (unit == "bohr") {
    conv_pos = 1.;
  } else {
    throw std::runtime_error(
        " StaticSite::WriteMpsLine: Unit conversion not known");
  }
  std::string output = "";
  output += (boost::format(" %1$2s %2$+1.7f %3$+1.7f %4$+1.7f Rank %5$d\n") %
             _element % (_pos(0) * conv_pos) % (_pos(1) * conv_pos) %
             (_pos(2) * conv_pos) % _rank)
                .str();
  output += (boost::format("    %1$+1.7f\n") % getCharge()).str();
  if (_rank > 0) {
    // Dipole z x y
    output += (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f\n") % _Q(3) %
               _Q(1) % _Q(2))
                  .str();
    if (_rank > 1) {
      // Quadrupole 20 21c 21s 22c 22s
      output +=
          (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f %4$+1.7f %5$+1.7f\n") %
           _Q(4) % _Q(5) % _Q(6) % _Q(7) % _Q(8))
              .str();
    }
  }
  // Polarizability
  output += writepolarization();
  return output;
}

void StaticSite::SetupCptTable(CptTable& table) const {
  table.addCol(_id, "index", HOFFSET(data, id));
  table.addCol(_element, "type", HOFFSET(data, element));

  table.addCol(_pos[0], "posX", HOFFSET(data, posX));
  table.addCol(_pos[1], "posY", HOFFSET(data, posY));
  table.addCol(_pos[2], "posZ", HOFFSET(data, posZ));

  table.addCol(_rank, "rank", HOFFSET(data, rank));

  table.addCol(_Q[0], "Q00", HOFFSET(data, Q00));
  table.addCol(_Q[1], "Q11c", HOFFSET(data, Q11c));
  table.addCol(_Q[2], "Q11s", HOFFSET(data, Q11s));
  table.addCol(_Q[3], "Q10", HOFFSET(data, Q10));
  table.addCol(_Q[4], "Q20", HOFFSET(data, Q20));
  table.addCol(_Q[5], "Q21c", HOFFSET(data, Q21c));
  table.addCol(_Q[6], "Q21s", HOFFSET(data, Q21s));
  table.addCol(_Q[7], "Q22c", HOFFSET(data, Q22c));
  table.addCol(_Q[8], "Q22s", HOFFSET(data, Q22s));
}

void StaticSite::WriteData(data& d) const {
  d.id = _id;
  d.element = const_cast<char*>(_element.c_str());
  d.posX = _pos[0];
  d.posY = _pos[1];
  d.posZ = _pos[2];

  d.rank = _rank;

  d.Q00 = _Q[0];
  d.Q11c = _Q[1];
  d.Q11s = _Q[2];
  d.Q10 = _Q[3];
  d.Q20 = _Q[4];
  d.Q21c = _Q[5];
  d.Q21s = _Q[6];
  d.Q22c = _Q[7];
  d.Q22s = _Q[8];
}

void StaticSite::ReadData(const data& d) {
  _id = d.id;
  _element = std::string(d.element);
  free(d.element);
  _pos[0] = d.posX;
  _pos[1] = d.posY;
  _pos[2] = d.posZ;

  _rank = d.rank;

  _Q[0] = d.Q00;
  _Q[1] = d.Q11c;
  _Q[2] = d.Q11s;
  _Q[3] = d.Q10;
  _Q[4] = d.Q20;
  _Q[5] = d.Q21c;
  _Q[6] = d.Q21s;
  _Q[7] = d.Q22c;
  _Q[8] = d.Q22s;
}

}  // namespace xtp
}  // namespace votca

ext: cc

max_stars_repo_name: fossabot/xtp

max_issues_repo_name: fossabot/xtp

max_forks_repo_name: fossabot/xtp


text:
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <fstream>
#include <numeric>

#include <boost/filesystem.hpp>
#include <glog/logging.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include "YamlFileIO.h"

Eigen::Matrix4d loadCameraPose(const std::string &file)
{
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    std::ifstream fs(file);
    if (!fs.is_open())
    {
        std::cerr << "Fail to open camera pose file " << file << std::endl;
        return T;
    }

    std::string line;
    int i = 0;
    while (!fs.eof())
    {
        line.clear();
        getline(fs, line);
        if (line.empty())
            continue;

        std::stringstream ss(line);
        std::string dummy_str;
        if (i < 4)
        {
            ss >> T(i, 0) >> dummy_str >> T(i, 1) >> dummy_str >> T(i, 2) >> dummy_str >> T(i, 3);
            i++;
        }
        else
        {
            std::cerr << "Invalid camera pose format!\n";
            break;
        }
    }
    fs.close();

    return T;
}

void evaluateBackpackExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c1(-0.03543, 0, -0.03543);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;
    Eigen::Matrix4d T_c0_c2_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c2_vec = Eigen::AngleAxisd(-180*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c2(0, 0, -0.07086);
    T_c0_c2_gt.block<3, 3>(0, 0) = c0_c2_vec.matrix();
    T_c0_c2_gt.block<3, 1>(0, 3) = t_c0_c2;
    Eigen::Matrix4d T_c0_c3_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c3_vec = Eigen::AngleAxisd(90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c3(0.03543, 0, -0.03543);
    T_c0_c3_gt.block<3, 3>(0, 0) = c0_c3_vec.matrix();
    T_c0_c3_gt.block<3, 1>(0, 3) = t_c0_c3;

    Eigen::Matrix4d T_l0_c0_gt = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_l1_c0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l1_c0_vec1 = Eigen::AngleAxisd(-15*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::AngleAxisd l1_c0_vec2 = Eigen::AngleAxisd(90*M_PI/180.0, Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d l1_c0_vec =  l1_c0_vec1.matrix()*l1_c0_vec2.matrix();
    Eigen::Vector3d t_l1_c0(0.58192, 0.0, -0.1954);
    T_l1_c0_gt.block<3,3>(0, 0) = l1_c0_vec;
    T_l1_c0_gt.block<3, 1>(0, 3) = l1_c0_vec* t_l1_c0;
    std::cout << "T_l1_c0_gt:\n" << T_l1_c0_gt << "\n";

    Eigen::Matrix4d T_l0_l1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l0_l1_vec1 = Eigen::AngleAxisd(-30*M_PI/180.0, Eigen::Vector3d(0,0,1));
    Eigen::AngleAxisd l0_l1_vec2 = Eigen::AngleAxisd(-75*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Matrix3d l0_l1_vec =  l0_l1_vec1.matrix()*l0_l1_vec2.matrix();
    Eigen::Vector3d t_l0_l1(-0.2604, 0, -0.34922);
    T_l0_l1_gt.block<3,3>(0, 0) = l0_l1_vec;
    T_l0_l1_gt.block<3, 1>(0, 3) = l0_l1_vec1.matrix() * t_l0_l1;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_camera2")
        T_gt = T_c0_c2_gt;
    if (eval_pattern == "camera0_to_camera3")
        T_gt = T_c0_c3_gt;
    if (eval_pattern == "lidar0_to_camera0")
        T_gt = T_l0_c0_gt;
    if (eval_pattern == "lidar1_to_camera0")
        T_gt = T_l1_c0_gt;
    if (eval_pattern == "lidar0_to_lidar1")
        T_gt = T_l0_l1_gt;

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(auto &T_ext : v_extrinsics){
        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        std::cout << "T_ext:\n" << T_ext << "\n";
        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles(ZYX): " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";

        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }
    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

void evaluateAntmanExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c1(0.05647, 0, -0.05647);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;
    Eigen::Matrix4d T_c0_c2_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c2_vec = Eigen::AngleAxisd(180*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Vector3d t_c0_c2(0, 0, -0.11294);
    T_c0_c2_gt.block<3, 3>(0, 0) = c0_c2_vec.matrix();
    T_c0_c2_gt.block<3, 1>(0, 3) = t_c0_c2;
    Eigen::Matrix4d T_c0_c3_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c3_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Vector3d t_c0_c3(-0.05647, 0, -0.05647);
    T_c0_c3_gt.block<3, 3>(0, 0) = c0_c3_vec.matrix();
    T_c0_c3_gt.block<3, 1>(0, 3) = t_c0_c3;

    Eigen::Matrix4d T_c3_l0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c3_l0_vec1 = Eigen::AngleAxisd(-M_PI/4.0, -Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd c3_l0_vec2 = Eigen::AngleAxisd(M_PI/2.0, Eigen::Vector3d::UnitX());
    Eigen::Matrix3d c3_l0_vec = c3_l0_vec2.matrix() * c3_l0_vec1.matrix();
    Eigen::Vector3d t_c3_l0(-0.03887, -0.03887, 0.0593);
    T_c3_l0_gt.block<3,3>(0, 0) = c3_l0_vec;
    T_c3_l0_gt.block<3,1>(0, 3) = t_c3_l0;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_camera2")
        T_gt = T_c0_c2_gt;
    if (eval_pattern == "camera0_to_camera3")
        T_gt = T_c0_c3_gt;
    // it is lidar0-camera0 actually
    if (eval_pattern == "camera0_to_lidar0")
        T_gt = T_c3_l0_gt;

    Eigen::Matrix3d R_gt = T_gt.block<3, 3>(0, 0);
    Eigen::Vector3d v_gt_angles = R_gt.eulerAngles(2,1,0) * 180.0/M_PI;
    LOG(INFO) << "GT Euler angles(ZYX): " << v_gt_angles.transpose() << "\n";

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(size_t i = 0; i < v_extrinsics.size(); i++){
        Eigen::Matrix4d T_ext = v_extrinsics[i];

        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles(ZYX): " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";

        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }

    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

void evaluateKaleidoExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(28*M_PI/180.0, Eigen::Vector3d(1,0,0));
    Eigen::Vector3d t_c0_c1(0, 0.028, 0);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;

    Eigen::Matrix4d T_c0_l0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_l0_vec = Eigen::AngleAxisd(-14*M_PI/180.0, Eigen::Vector3d(1,0,0));
    Eigen::Vector3d t_c0_l0(0, -0.014, 0.020);
    T_c0_l0_gt.block<3, 3>(0, 0) = c0_l0_vec.matrix();
    T_c0_l0_gt.block<3, 1>(0, 3) = t_c0_l0;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_lidar0")
        T_gt = T_c0_l0_gt;

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(auto &T_ext : v_extrinsics){
        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles: " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";
        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }
    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

// compare several camera poses or extrinsic params, to evaluate params consistency
int main(int argc, char** argv){

    // This exe is used to analyze the exytrinsic calibration repeatability
    if(argc < 2){
        std::cerr <<"Usage: ./test_eval_extrinsics [input_folder] [file_prefix]\n";
        std::cout << "[input_folder]: contains many extrinsic calibration result.\n";
        std::cout << "[file_prefix]: extrinsic file prefix, different sensor configuration results in different extrinsic file.\n";
        return -1;
    }

    std::string dataset_folder(argv[1]);
    std::string file_prefix(argv[2]);

    std::vector<Eigen::Matrix4d> v_extrinsics;

    bool b_eval_backpack = false, b_eval_antman = false, b_eval_kaleido = false;
    if (dataset_folder.find("backpack") != std::string::npos)
        b_eval_backpack = true;
    if (dataset_folder.find("antman") != std::string::npos)
        b_eval_antman = true;
    if (dataset_folder.find("kaleido") != std::string::npos)
        b_eval_kaleido = true;

    for (const auto & entry : boost::filesystem::directory_iterator(dataset_folder)){
        if(!boost::filesystem::is_directory(entry))
            continue;

        std::string extrin_filepath = entry.path().string() + "/" +file_prefix + ".yml";
        Eigen::Matrix4d T_ext = Eigen::Matrix4d::Identity();
        if (common::loadExtFileOpencv(extrin_filepath, T_ext)){
            LOG(INFO) << "Read extrinsic parameters from " << extrin_filepath;
        }else{
            LOG(ERROR) << "Fail to load " << extrin_filepath;
            continue;
        }
        v_extrinsics.emplace_back(T_ext);
    }

    if (b_eval_backpack){
    evaluateBackpackExt(v_extrinsics, file_prefix);
    }else{
        if (b_eval_antman){
            evaluateAntmanExt(v_extrinsics, file_prefix);
        }else{
            evaluateKaleidoExt(v_extrinsics, file_prefix);
        }
    }
    return 0;
}

ext: cpp


text:
/*********************************************************************
 * Software License Agreement (CC BY-NC-SA 4.0 License)
 *
 *  Copyright (c) 2015, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  This work is licensed under the Creative Commons
 *  Attribution-NonCommercial-ShareAlike 4.0 International License.
 *
 *  To view a copy of this license, visit
 *  http://creativecommons.org/licenses/by-nc-sa/4.0/
 *  or send a letter to
 *  Creative Commons, 444 Castro Street, Suite 900,
 *  Mountain View, California, 94041, USA.
 *********************************************************************/

/*
 * @author Sammy Pfeiffer
 */

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream>
#include <boost/assign/list_of.hpp>

// Based on the ros tutorial on transforming opencv images to Image messages

sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img){
    sensor_msgs::CameraInfo cam_info_msg;
    cam_info_msg.header.frame_id = img->header.frame_id;
    // Fill image size
    cam_info_msg.height = img->height;
    cam_info_msg.width = img->width;
    // Add the most common distortion model as sensor_msgs/CameraInfo says
    cam_info_msg.distortion_model = "plumb_bob";
    // Don't let distorsion matrix be empty
    cam_info_msg.D.resize(5, 0.0);
    // Give a reasonable default intrinsic camera matrix
    cam_info_msg.K = boost::assign::list_of(1.0) (0.0) (img->width/2.0)
                                           (0.0) (1.0) (img->height/2.0)
                                           (0.0) (0.0) (1.0);
    // Give a reasonable default rectification matrix
    cam_info_msg.R = boost::assign::list_of (1.0) (0.0) (0.0)
                                            (0.0) (1.0) (0.0)
                                            (0.0) (0.0) (1.0);
    // Give a reasonable default projection matrix
    cam_info_msg.P = boost::assign::list_of (1.0) (0.0) (img->width/2.0) (0.0)
                                            (0.0) (1.0) (img->height/2.0) (0.0)
                                            (0.0) (0.0) (1.0) (0.0);
    return cam_info_msg;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    ros::NodeHandle _nh("~"); // to get the private params
    image_transport::ImageTransport it(nh);
    image_transport::CameraPublisher pub = it.advertiseCamera("camera", 1);

    // provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number of device, (e.g.: 0 would be /dev/video0)
    std::string video_stream_provider;
    cv::VideoCapture cap;
    if (_nh.getParam("video_stream_provider", video_stream_provider)){
        ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider);
        // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected)
        // treat is as a number and act accordingly so we open up the videoNUMBER device
        if (video_stream_provider.size() < 4){
            ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider);
            cap.open(atoi(video_stream_provider.c_str()));
        }
        else{
            ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider);
            cap.open(video_stream_provider);
        }
    }
    else{
        ROS_ERROR("Failed to get param 'video_stream_provider'");
        return -1;
    }

    std::string camera_name;
    _nh.param("camera_name", camera_name, std::string("camera"));
    ROS_INFO_STREAM("Camera name: " << camera_name);

    int fps;
    _nh.param("fps", fps, 240);
    ROS_INFO_STREAM("Throttling to fps: " << fps);

    std::string frame_id;
    _nh.param("frame_id", frame_id, std::string("camera"));
    ROS_INFO_STREAM("Publishing with frame_id: " << frame_id);

    std::string camera_info_url;
    _nh.param("camera_info_url", camera_info_url, std::string(""));
    ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url << "'");

    bool flip_horizontal;
    _nh.param("flip_horizontal", flip_horizontal, false);
    ROS_INFO_STREAM("Flip horizontal image is : " << ((flip_horizontal)?"true":"false"));

    bool flip_vertical;
    _nh.param("flip_vertical", flip_vertical, false);
    ROS_INFO_STREAM("Flip flip_vertical image is : " << ((flip_vertical)?"true":"false"));

    // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#void flip(InputArray src, OutputArray dst, int flipCode)
    // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1
    bool flip_image = true;
    int flip_value;
    if (flip_horizontal && flip_vertical)
        flip_value = 0; // flip both, horizontal and vertical
    else if (flip_horizontal)
        flip_value = 1;
    else if (flip_vertical)
        flip_value = -1;
    else
        flip_image = false;

    if(!cap.isOpened()){
        ROS_ERROR_STREAM("Could not open the stream.");
        return -1;
    }

    ROS_INFO_STREAM("Opened the stream, starting to publish.");

    cv::Mat frame;
    sensor_msgs::ImagePtr msg;
    sensor_msgs::CameraInfo cam_info_msg;
    std_msgs::Header header;
    header.frame_id = frame_id;
    camera_info_manager::CameraInfoManager cam_info_manager(nh, camera_name, camera_info_url);
    // Get the saved camera info if any
    cam_info_msg = cam_info_manager.getCameraInfo();

    ros::Rate r(fps);
    while (nh.ok()) {
        cap >> frame;
        if (pub.getNumSubscribers() > 0){
            // Check if grabbed frame is actually full with some content
            if(!frame.empty()) {
                // Flip the image if necessary
                if (flip_image)
                    cv::flip(frame, frame, flip_value);
                msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg();
                // Create a default camera info if we didn't get a stored one on initialization
                if (cam_info_msg.distortion_model == ""){
                    ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info.");
                    cam_info_msg = get_default_camera_info_from_image(msg);
                    cam_info_manager.setCameraInfo(cam_info_msg);
                }
                // The timestamps are in sync thanks to this publisher
                pub.publish(*msg, cam_info_msg, ros::Time::now());
            }

            ros::spinOnce();
        }
        r.sleep();
    }
}

max_stars_repo_name: robertsatya/ros-bot

max_issues_repo_name: robertsatya/ros-bot

max_forks_repo_name: zhudelong/HawkEye


text:
#pragma once
/*
**  Copyright (C) 2013 Aldebaran Robotics
**  See COPYING for the license
*/

#ifndef _QITYPE_PROPERTY_HPP_
#define _QITYPE_PROPERTY_HPP_

#include <boost/function.hpp>
#include <qitype/signal.hpp>

#ifdef _MSC_VER
#  pragma warning( push )
   // needs to have dll-interface to be used by clients
#  pragma warning( disable: 4251 )
   // non dll-interface class * used as base for dll-interface class
#  pragma warning( disable: 4275 )
#endif

namespace qi
{

  /** Type-erased virtual interface implemented by all Property classes.
  */
  class QITYPE_API PropertyBase
  {
  public:
    virtual ~PropertyBase() {}
    virtual SignalBase* signal() = 0;
    //TODO: why not AutoAnyReference?
    virtual void setValue(AutoAnyReference value) = 0;
    virtual AnyValue value() const = 0;
  };

  template<typename T>
  class PropertyImpl: public SignalF<void(const T&)>, public PropertyBase
  {
  public:
    /** Setter called with storage containing old value, and new value
    *  Returns true to invoke subscribers, false to 'abort' the update.
    */
    typedef boost::function<bool (T&, const T&)> Setter;
    typedef boost::function<T(const T&)> Getter;
    typedef SignalF<void(const T&)> SignalType;
    typedef T PropertyType;
    /**
     * @param getter value getter, default to reading _value
     * @param setter value setter, what it returns will be written to
     *        _value. If it throws AbortUpdate, update operation will
     *        be silently aborted (subscribers will not be called)
    */
    PropertyImpl(Getter getter = Getter(), Setter setter = Setter(),
      SignalBase::OnSubscribers onsubscribe = SignalBase::OnSubscribers());
    virtual ~PropertyImpl() {}
    T get() const;
    void set(const T& v);
    void operator = (const T& v) { set(v);}
  protected:
    Getter _getter;
    Setter _setter;
    T      _value;
  };

  template<typename T>
  class Property: public PropertyImpl<T>
  {
  public:
    typedef PropertyImpl<T> ImplType;
    typedef typename ImplType::Getter Getter;
    typedef typename ImplType::Setter Setter;
    Property(Getter getter = Getter(), Setter setter = Setter(),
      SignalBase::OnSubscribers onsubscribe = SignalBase::OnSubscribers())
    : PropertyImpl<T>(getter, setter, onsubscribe)
    {}
    virtual SignalBase* signal() { return this;}
    virtual void setValue(AutoAnyReference value)  { PropertyImpl<T>::set(value.to<T>());}
    virtual AnyValue value() const { return AnyValue::from(PropertyImpl<T>::get());}
  };

  template<>
  class QITYPE_API Property<AnyValue>: public PropertyImpl<AnyValue>
  {
  public:

    Property(Getter getter = Getter(), Setter setter = Setter(),
      SignalBase::OnSubscribers onsubscribe = SignalBase::OnSubscribers())
    : PropertyImpl<AnyValue>(getter, setter, onsubscribe)
    {
    }
    virtual SignalBase* signal() { return this;}
    virtual void setValue(AutoAnyReference value)  { set(AnyValue(value, false, false));}
    virtual AnyValue value() const { return get();}
  };

  /// Type-erased property, simulating a typed property but using AnyValue.
  class QITYPE_API GenericProperty: public Property<AnyValue>
  {
  public:
    GenericProperty(TypeInterface* type, Getter getter = Getter(), Setter setter = Setter())
    :Property<AnyValue>(getter, setter)
    , _type(type)
    { // Initialize with default value for given type
      set(AnyValue(_type));
      std::vector<TypeInterface*> types(&_type, &_type + 1);
      _setSignature(makeTupleSignature(types));
    }
    virtual void setValue(AutoAnyReference value)  { set(AnyValue(value, false, false));}
    void set(const AnyValue& v);
    virtual qi::Signature signature() const {
      return makeTupleSignature(std::vector<TypeInterface*>(&_type, &_type + 1));
    }
  private:
    TypeInterface* _type;
  };

}

#include <qitype/details/property.hxx>

#ifdef _MSC_VER
#  pragma warning( pop )
#endif

#endif  // _QITYPE_PROPERTY_HPP_

ext: hpp


text:
#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sophus/so3.h>
#include <sophus/se3.h>

#include <stdio.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv2/core/eigen.hpp>

using namespace std;

cv::Mat convertRt2T(const cv::Mat &R, const cv::Mat &t)
{
    cv::Mat T = (cv::Mat_<double>(4, 4) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0),
            0, 0, 0, 1);
    return T;
}

Sophus::SE3 transT_cv2sophus(const cv::Mat &T_cv)
{
    Eigen::Matrix3d R_eigen;
    cv::cv2eigen(T_cv(cv::Rect2d(0, 0, 3, 3)), R_eigen);
    Eigen::Vector3d t_eigen(T_cv.at<double>(0, 3), T_cv.at<double>(1, 3), T_cv.at<double>(2, 3));
    Sophus::SE3 SE3_Rt(R_eigen, t_eigen);
    return SE3_Rt;
}

cv::Mat transT_sophus2cv(const Sophus::SE3 &T_sophus)
{
    Eigen::Vector3d eigen_t(T_sophus.translation());
    Eigen::Matrix3d eigen_R(T_sophus.rotation_matrix());

    cv::Mat cv_t, cv_R;
    eigen2cv(eigen_t, cv_t);
    eigen2cv(eigen_R, cv_R);

    return convertRt2T(cv_R, cv_t);
}

int main(int argc, char **argv)
{
    // -------------------- Init value from OpenCV --------------------
    cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat rvec = (cv::Mat_<double>(3, 1) << 0.3, 0.1, -0.5);
    cv::Rodrigues(rvec, R);
    cv::Mat tvec = (cv::Mat_<double>(3, 1) << 1, 2, 3);

    cout << "----- Initial value in OpenCV form -----" << endl;
    cout << "R=\n"
         << R << endl;
    cout << "R_vec = " << rvec.t() << endl;
    cout << "t=" << tvec.t() << endl;

    // -------------------- Convert to Sophus --------------------
    Sophus::SE3 T = Sophus::SE3(
            Sophus::SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
            Eigen::Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0)));
    cout << "Change form to Sophus:" << endl;
    cout << "Sophus::SE3 T = \n"
         << T << endl;

    // -------------------- Then convert to Eigen --------------------
    cout << "\n\nChange back to Eigen:" << endl;
    Eigen::Vector3d eigen_t(T.translation());
    Eigen::Matrix3d eigen_R(T.rotation_matrix());
    cout << eigen_t << endl;
    cout << eigen_R << endl;

    // -------------------- Then convert to OpenCV --------------------
    cout << "\n\nChange back to OpenCV:" << endl;
    cv::Mat cv_t, cv_R;
    eigen2cv(eigen_t, cv_t);
    eigen2cv(eigen_R, cv_R);
    cout << cv_t << endl;
    cout << cv_R << endl;

    // -------------------- Direct trans from T_cv to T_Sophus --------------------
    cv::Mat T_cv = convertRt2T(R, tvec);
    Sophus::SE3 T_SE3 = transT_cv2sophus(T_cv);
    cout << "\nDirect trans from T_cv to T_Sophus: \n"
         << T_SE3 << endl;
    // -------------------- Direct trans from T_sophus to T_cv --------------------
    T_cv = transT_sophus2cv(T_SE3);
    cout << "\nDirect trans from T_sophus to T_cv:\n"
         << T_cv << endl;
    return 0;
}


text:
// (C) Copyright Gert-Jan de Vos and Jan Wilmans 2013.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// Repository at: https://github.com/djeedjay/DebugViewPP/

#include "stdafx.h"
#include <boost/algorithm/string.hpp>
#include <utility>
#include <atlstr.h>
#include "Win32/Utilities.h"
#include "CobaltFusion/Str.h"
#include "CobaltFusion/AtlWinExt.h"
#include "CobaltFusion/fusionassert.h"
#include "DebugView++Lib/LogFilter.h"
#include "DebugView++Lib/LogSource.h"
#include "resource.h"
#include "SourceDlg.h"
#include "SourcesDlg.h"

namespace fusion {
namespace debugviewpp {

BEGIN_MSG_MAP2(CSourcesDlg)
    MSG_WM_INITDIALOG(OnInitDialog)
    NOTIFY_CODE_HANDLER_EX(PIN_CLICK, OnClickItem)
    COMMAND_ID_HANDLER_EX(IDCANCEL, OnCancel)
    COMMAND_ID_HANDLER_EX(IDOK, OnOk)
    COMMAND_ID_HANDLER_EX(IDADD, OnAdd)
    REFLECT_NOTIFICATIONS()
    CHAIN_MSG_MAP(CDialogResize<CSourcesDlg>)
END_MSG_MAP()

CSourcesDlg::CSourcesDlg(std::vector<SourceInfo> sourceInfos) :
    m_sourceInfos(std::move(sourceInfos))
{
}

void CSourcesDlg::OnException()
{
    FUSION_REPORT_EXCEPTION("Unknown Exception");
}

void CSourcesDlg::OnException(const std::exception& ex)
{
    FUSION_REPORT_EXCEPTION(ex.what());
}

void CSourcesDlg::UpdateGrid()
{
    m_grid.DeleteAllItems();
    for (auto& sourceInfo : m_sourceInfos)
    {
        int item = m_grid.GetItemCount();
        m_grid.InsertItem(item, PropCreateCheckButton(L"", sourceInfo.enabled));
        m_grid.SetSubItem(item, 1, PropCreateReadOnlyItem(L"", sourceInfo.description.c_str()));
        m_grid.SetSubItem(item, 2, PropCreateReadOnlyItem(L"", WStr(SourceTypeToString(sourceInfo.type))));
        if (sourceInfo.type == SourceType::System)
        {
            m_grid.SetSubItem(item, 3, PropCreateReadOnlyItem(L"", L""));
        }
        else
        {
            m_grid.SetSubItem(item, 3, PropCreateReadOnlyItem(L"", L"x"));
        }
    }
}

BOOL CSourcesDlg::OnInitDialog(CWindow /*wndFocus*/, LPARAM /*lInitParam*/)
{
    m_grid.SubclassWindow(GetDlgItem(IDC_SOURCES_GRID));
    m_grid.InsertColumn(0, L"", LVCFMT_LEFT, 32, 0);
    m_grid.InsertColumn(1, L"Source description", LVCFMT_LEFT, 280, 0);
    m_grid.InsertColumn(2, L"Type", LVCFMT_LEFT, 100, 0);
    m_grid.InsertColumn(3, L"", LVCFMT_LEFT, 16, 0);
    m_grid.SetExtendedGridStyle(PGS_EX_SINGLECLICKEDIT);

    UpdateGrid();
    CenterWindow(GetParent());
    DlgResize_Init();
    return TRUE;
}

LRESULT CSourcesDlg::OnClickItem(NMHDR* pnmh)
{
    auto& nmhdr = *reinterpret_cast<NMPROPERTYITEM*>(pnmh);

    int iItem = -1;
    int iSubItem = -1;
    if ((m_grid.FindProperty(nmhdr.prop, iItem, iSubItem) != 0) && iSubItem == 3)
    {
        if (GetSourceType(iItem) != SourceType::System)
        {
            m_grid.DeleteItem(iItem);
            m_sourceInfos.erase(m_sourceInfos.begin() + iItem);
            return TRUE;
        }
    }
    return FALSE;
}

bool CSourcesDlg::GetSourceEnable(int iItem) const
{
    CComVariant val;
    GetGridItem<CPropertyCheckButtonItem>(m_grid, iItem, 0).GetValue(&val);
    return val.boolVal != VARIANT_FALSE;
}

std::wstring CSourcesDlg::GetSourceText(int iItem) const
{
    return GetGridItemText(m_grid, iItem, 1);
}

SourceType::type CSourcesDlg::GetSourceType(int iItem) const
{
    return StringToSourceType(Str(GetGridItemText(m_grid, iItem, 2)));
}

void CSourcesDlg::OnCancel(UINT /*uNotifyCode*/, int nID, CWindow /*wndCtl*/)
{
    m_sourceInfos.clear();
    EndDialog(nID);
}

void CSourcesDlg::OnOk(UINT /*uNotifyCode*/, int nID, CWindow /*wndCtl*/)
{
    EndDialog(nID);
}

void CSourcesDlg::OnAdd(UINT /*uNotifyCode*/, int /*nID*/, CWindow /*wndCtl*/)
{
    CSourceDlg dlg(L"UDP port 2020", SourceType::Udp, L"192.168.1.1", 2020);
    if (dlg.DoModal() != IDOK)
    {
        return;
    }

    auto info = SourceInfo(dlg.GetName(), dlg.GetSourceType(), dlg.GetAddress(), dlg.GetPort());
    info.enabled = true;
    m_sourceInfos.push_back(info);
    UpdateGrid();
}

std::vector<SourceInfo> CSourcesDlg::GetSourceInfos()
{
    return m_sourceInfos;
}

} // namespace debugviewpp
} // namespace fusion

max_stars_repo_name: wbaby/DebugViewPP

max_issues_repo_name: wbaby/DebugViewPP

max_forks_repo_name: wbaby/DebugViewPP


text:
//  Copyright (c) 2001-2011 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(BOOST_SPIRIT_KARMA_AUTO_NOV_29_2009_0339PM)
#define BOOST_SPIRIT_KARMA_AUTO_NOV_29_2009_0339PM

#if defined(_MSC_VER)
#pragma once
#endif

#include <boost/mpl/bool.hpp>
#include <boost/spirit/home/karma/auto/create_generator.hpp>
#include <boost/spirit/home/karma/delimit_out.hpp>
#include <boost/spirit/home/karma/domain.hpp>
#include <boost/spirit/home/karma/generator.hpp>
#include <boost/spirit/home/karma/meta_compiler.hpp>
#include <boost/spirit/home/support/assert_msg.hpp>
#include <boost/spirit/home/support/common_terminals.hpp>
#include <boost/spirit/home/support/container.hpp>
#include <boost/spirit/home/support/detail/hold_any.hpp>
#include <boost/spirit/home/support/info.hpp>

///////////////////////////////////////////////////////////////////////////////
namespace boost {
namespace spirit {
///////////////////////////////////////////////////////////////////////////
// Enablers
///////////////////////////////////////////////////////////////////////////
template <>
struct use_terminal<karma::domain, tag::auto_> // enables auto_
    : mpl::true_ {};

template <typename A0>
struct use_terminal<karma::domain // enables auto_(...)
                    ,
                    terminal_ex<tag::auto_, fusion::vector1<A0>>> : mpl::true_ {
};

template <>                                           // enables auto_(f)
struct use_lazy_terminal<karma::domain, tag::auto_, 1 /*arity*/
                         > : mpl::true_ {};

} // namespace spirit
} // namespace boost

///////////////////////////////////////////////////////////////////////////////
namespace boost {
namespace spirit {
namespace karma {
#ifndef BOOST_SPIRIT_NO_PREDEFINED_TERMINALS
using spirit::auto_;
#endif
using spirit::auto_type;

///////////////////////////////////////////////////////////////////////////
template <typename Modifiers>
struct auto_generator : generator<auto_generator<Modifiers>> {
  typedef mpl::int_<generator_properties::all_properties> properties;

  template <typename Context, typename Unused> struct attribute {
    typedef spirit::basic_hold_any<char> type;
  };

  auto_generator(Modifiers const &modifiers) : modifiers_(modifiers) {}

  // auto_generator has an attached attribute
  template <typename OutputIterator, typename Context, typename Delimiter,
            typename Attribute>
  bool generate(OutputIterator &sink, Context &context, Delimiter const &d,
                Attribute const &attr) const {
    return compile<karma::domain>(create_generator<Attribute>(), modifiers_)
        .generate(sink, context, d, attr);
  }

  // this auto_generator has no attribute attached, it needs to have been
  // initialized from a value/variable
  template <typename OutputIterator, typename Context, typename Delimiter>
  static bool generate(OutputIterator &, Context &, Delimiter const &,
                       unused_type) {
    // It is not possible (doesn't make sense) to use auto_ generators
    // without providing any attribute, as the generator doesn't 'know'
    // what to output. The following assertion fires if this situation
    // is detected in your code.
    BOOST_SPIRIT_ASSERT_FAIL(OutputIterator, auto_not_usable_without_attribute,
                             ());
    return false;
  }

  template <typename Context> info what(Context & /*context*/) const {
    return info("auto_");
  }

  Modifiers modifiers_;
};

///////////////////////////////////////////////////////////////////////////
template <typename T, typename Modifiers>
struct lit_auto_generator : generator<lit_auto_generator<T, Modifiers>> {
  typedef mpl::int_<generator_properties::all_properties> properties;

  template <typename Context, typename Unused> struct attribute {
    typedef unused_type type;
  };

  lit_auto_generator(typename add_reference<T>::type t,
                     Modifiers const &modifiers)
      : t_(t),
        generator_(compile<karma::domain>(create_generator<T>(), modifiers)) {}

  // auto_generator has an attached attribute
  template <typename OutputIterator, typename Context, typename Delimiter,
            typename Attribute>
  bool generate(OutputIterator &sink, Context &context, Delimiter const &d,
                Attribute const &) const {
    return generator_.generate(sink, context, d, t_);
  }

  template <typename Context> info what(Context & /*context*/) const {
    return info("auto_");
  }

  typedef typename spirit::result_of::create_generator<T>::type generator_type;

  typedef
      typename spirit::result_of::compile<karma::domain, generator_type,
                                          Modifiers>::type generator_impl_type;

  T t_;
  generator_impl_type generator_;

  // silence MSVC warning C4512: assignment operator could not be generated
  BOOST_DELETED_FUNCTION(
      lit_auto_generator &operator=(lit_auto_generator const &))
};

///////////////////////////////////////////////////////////////////////////
// Generator generators: make_xxx function (objects)
///////////////////////////////////////////////////////////////////////////

// auto_
template <typename Modifiers> struct make_primitive<tag::auto_, Modifiers> {
  typedef auto_generator<Modifiers> result_type;

  result_type operator()(unused_type, Modifiers const &modifiers) const {
    return result_type(modifiers);
  }
};

// auto_(...)
template <typename Modifiers, typename A0>
struct make_primitive<terminal_ex<tag::auto_, fusion::vector1<A0>>, Modifiers> {
  typedef typename add_const<A0>::type const_attribute;

  typedef lit_auto_generator<const_attribute, Modifiers> result_type;

  template <typename Terminal>
  result_type operator()(Terminal const &term,
                         Modifiers const &modifiers) const {
    return result_type(fusion::at_c<0>(term.args), modifiers);
  }
};

} // namespace karma
} // namespace spirit
} // namespace boost

#endif

max_stars_repo_name: henrywarhurst/matrix

max_issues_repo_name: henrywarhurst/matrix

max_forks_repo_name: henrywarhurst/matrix


text:
/* value_printer_test.cpp                                 -*- C++ -*-
   Rémi Attab (remi.attab@gmail.com), 25 Apr 2015
   FreeBSD-style copyright and disclaimer apply
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define REFLECT_USE_EXCEPTIONS 1

#include "printer_utils.h"
#include "test_types.h"

#include <boost/test/unit_test.hpp>
#include <fstream>

using namespace reflect;
using namespace reflect::json;

/******************************************************************************/
/* TESTS                                                                      */
/******************************************************************************/

BOOST_AUTO_TEST_CASE(test_full)
{
    Basics value;
    Basics::construct(value);
    checkFile("value_printer_full.json", value);
}

BOOST_AUTO_TEST_CASE(test_compact)
{
    Basics value;
    Basics::construct(value);
    checkFile("value_printer_compact.json", value, true);
}

max_stars_repo_name: unbornchikken/reflect-cpp

max_issues_repo_name: unbornchikken/reflect-cpp

max_forks_repo_name: unbornchikken/reflect-cpp


text:
// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_FOR_EACH_RANGE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_FOR_EACH_RANGE_HPP

#include <type_traits>
#include <utility>

#include <boost/concept/requires.hpp>
#include <boost/core/addressof.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tag_cast.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/type_traits.hpp>

#include <boost/geometry/views/box_view.hpp>
#include <boost/geometry/views/segment_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace for_each
{

template <typename Point>
struct fe_range_point
{
    template <typename Functor>
    static inline bool apply(Point& point, Functor&& f)
    {
        Point* ptr = boost::addressof(point);
        return f(std::pair<Point*, Point*>(ptr, ptr + 1));
    }
};

template <typename Segment>
struct fe_range_segment
{
    template <typename Functor>
    static inline bool apply(Segment& segment, Functor&& f)
    {
        return f(segment_view<typename std::remove_const<Segment>::type>(segment));
    }
};

template <typename Range>
struct fe_range_range
{
    template <typename Functor>
    static inline bool apply(Range& range, Functor&& f)
    {
        return f(range);
    }
};

template <typename Polygon>
struct fe_range_polygon
{
    template <typename Functor>
    static inline bool apply(Polygon& polygon, Functor&& f)
    {
        return f(exterior_ring(polygon));

        // TODO: If some flag says true, also do the inner rings.
        // for convex hull, it's not necessary
    }
};

template <typename Box>
struct fe_range_box
{
    template <typename Functor>
    static inline bool apply(Box& box, Functor&& f)
    {
        return f(box_view<typename std::remove_const<Box>::type>(box));
    }
};

template <typename Multi, typename SinglePolicy>
struct fe_range_multi
{
    template <typename Functor>
    static inline bool apply(Multi& multi, Functor&& f)
    {
        auto const end = boost::end(multi);
        for (auto it = boost::begin(multi); it != end; ++it)
        {
            if (! SinglePolicy::apply(*it, f))
            {
                return false;
            }
        }
        return true;
    }
};

}} // namespace detail::for_each
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct for_each_range
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not or not yet implemented for this Geometry type.",
        Geometry, Tag);
};

template <typename Point>
struct for_each_range<Point, point_tag>
    : detail::for_each::fe_range_point<Point>
{};

template <typename Segment>
struct for_each_range<Segment, segment_tag>
    : detail::for_each::fe_range_segment<Segment>
{};

template <typename Linestring>
struct for_each_range<Linestring, linestring_tag>
    : detail::for_each::fe_range_range<Linestring>
{};

template <typename Ring>
struct for_each_range<Ring, ring_tag>
    : detail::for_each::fe_range_range<Ring>
{};

template <typename Polygon>
struct for_each_range<Polygon, polygon_tag>
    : detail::for_each::fe_range_polygon<Polygon>
{};

template <typename Box>
struct for_each_range<Box, box_tag>
    : detail::for_each::fe_range_box<Box>
{};

template <typename MultiPoint>
struct for_each_range<MultiPoint, multi_point_tag>
    : detail::for_each::fe_range_range<MultiPoint>
{};

template <typename Geometry>
struct for_each_range<Geometry, multi_linestring_tag>
    : detail::for_each::fe_range_multi
        <
            Geometry,
            detail::for_each::fe_range_range
                <
                    util::transcribe_const_t
                        <
                            Geometry,
                            typename boost::range_value<Geometry>::type
                        >
                >
        >
{};

template <typename Geometry>
struct for_each_range<Geometry, multi_polygon_tag>
    : detail::for_each::fe_range_multi
        <
            Geometry,
            detail::for_each::fe_range_polygon
                <
                    util::transcribe_const_t
                        <
                            Geometry,
                            typename boost::range_value<Geometry>::type
                        >
                >
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace detail
{

// Currently for Polygons p is checked only for exterior ring
// Should this function be renamed?
template <typename Geometry, typename UnaryPredicate>
inline bool all_ranges_of(Geometry const& geometry, UnaryPredicate p)
{
    return dispatch::for_each_range<Geometry const>::apply(geometry, p);
}

// Currently for Polygons p is checked only for exterior ring
// Should this function be renamed?
template <typename Geometry, typename UnaryPredicate>
inline bool any_range_of(Geometry const& geometry, UnaryPredicate p)
{
    return ! dispatch::for_each_range<Geometry const>::apply(geometry,
                [&](auto&& range)
                {
                    return ! p(range);
                });
}

// Currently for Polygons p is checked only for exterior ring
// Should this function be renamed?
template <typename Geometry, typename UnaryPredicate>
inline bool none_range_of(Geometry const& geometry, UnaryPredicate p)
{
    return dispatch::for_each_range<Geometry const>::apply(geometry,
                [&](auto&& range)
                {
                    return ! p(range);
                });
}

// Currently for Polygons f is called only for exterior ring
// Should this function be renamed?
template <typename Geometry, typename Functor>
inline Functor for_each_range(Geometry const& geometry, Functor f)
{
    dispatch::for_each_range<Geometry const>::apply(geometry,
        [&](auto&& range)
        {
            f(range);
            // TODO: Implement separate function?
            return true;
        });
    return f;
}

}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_FOR_EACH_RANGE_HPP


text:
// Copyright (C) 2008  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/test/tester.h>
#include <dlib/logger.h>
/////////////////////////////
#include "infotestdata.h"
//////////////////////////
#include <sqlitestathelper.h>
////////////////////////////////////////////
// This is called an unnamed-namespace and it has the effect of making everything
// inside this file "private" so that everything you declare will have static linkage.
// Thus we won't have any multiply defined symbol errors coming out of the linker when
// we try to compile the test suite.
using namespace std;
using namespace test;
using namespace info;
using namespace dlib;
namespace
{
	// Declare the logger we will use in this test.  The name of the logger
	// should start with "test."
	static dlib::logger dlog("test.sqlitestathelper");
	//
	class sqlitestatmanager_tester : public tester
	{
		/*!
		WHAT THIS OBJECT REPRESENTS
		This object represents a unit test.  When it is constructed
		it adds itself into the testing framework.
		!*/
	public:
		sqlitestatmanager_tester() :
			tester(
				"sqlitestatmanager",       // the command line argument name for this test
				"Run sqlitestatmanager tests.", // the command line argument description
				0                     // the number of command line arguments for this test
			)
		{
		}

		void perform_test()
		{
			// This message gets logged to the file debug.txt if the user has enabled logging by
			// supplying the -d option on the command line (and they haven't set the logging level
			// to something higher than LINFO).
			dlog << LINFO << "ENTERING sqlitestatmanage tests...";
			//
			std::string dbName;
			InfoTestData::get_database_filename(dbName);
			DLIB_TEST_MSG(!dbName.empty(), "Database filename cannot be empty");
			SQLiteStatHelper oMan(dbName);
			IStoreHelper *pHelper = &oMan;
			DLIB_TEST_MSG(pHelper->is_valid(), "StatDataManager is not valid");
			//
			std::string setName;
			InfoTestData::get_conso_name(setName);
			DLIB_TEST_MSG(!setName.empty(), "set name cannot be empty");

		}// perform_test
	};

	// Create an instance of this object.  Doing this causes this test
	// to be automatically inserted into the testing framework whenever this cpp file
	// is linked into the project.  Note that since we are inside an unnamed-namespace
	// we won't get any linker errors about the symbol a being defined multiple times.
	sqlitestatmanager_tester a;

}

max_stars_repo_name: boubad/CygProjects

max_issues_repo_name: boubad/CygProjects

max_forks_repo_name: boubad/CygProjects


text:
/*
 *          Copyright Andrey Semashev 2007 - 2015.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file   attr_value_visitation.cpp
 * \author Andrey Semashev
 * \date   21.01.2009
 *
 * \brief  This header contains tests for the attribute value extraction helpers.
 */

#define BOOST_TEST_MODULE attr_value_visitation

#include <string>
#include <boost/mpl/vector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/log/attributes/value_visitation.hpp>
#include <boost/log/attributes/constant.hpp>
#include <boost/log/attributes/attribute_set.hpp>
#include <boost/log/attributes/attribute_value_set.hpp>
#include "char_definitions.hpp"

namespace mpl = boost::mpl;
namespace logging = boost::log;
namespace attrs = logging::attributes;

namespace {

    // The receiver functional object that verifies the extracted attribute values
    struct my_receiver
    {
        typedef void result_type;

        enum type_expected
        {
            none_expected,
            int_expected,
            double_expected,
            string_expected
        };

        my_receiver() : m_Expected(none_expected), m_Int(0), m_Double(0.0) {}

        void set_expected()
        {
            m_Expected = none_expected;
        }
        void set_expected(int value)
        {
            m_Expected = int_expected;
            m_Int = value;
        }
        void set_expected(double value)
        {
            m_Expected = double_expected;
            m_Double = value;
        }
        void set_expected(std::string const& value)
        {
            m_Expected = string_expected;
            m_String = value;
        }

        // Implement visitation logic for all supported types
        void operator() (int const& value)
        {
            BOOST_CHECK_EQUAL(m_Expected, int_expected);
            BOOST_CHECK_EQUAL(m_Int, value);
        }
        void operator() (double const& value)
        {
            BOOST_CHECK_EQUAL(m_Expected, double_expected);
            BOOST_CHECK_CLOSE(m_Double, value, 0.001);
        }
        void operator() (std::string const& value)
        {
            BOOST_CHECK_EQUAL(m_Expected, string_expected);
            BOOST_CHECK_EQUAL(m_String, value);
        }
        void operator() (char value)
        {
            // This one should not be called
            BOOST_ERROR("The unexpected operator() has been called");
        }

    private:
        type_expected m_Expected;
        int m_Int;
        double m_Double;
        std::string m_String;
    };

} // namespace

// The test checks invokers specialized on a single attribute value type
BOOST_AUTO_TEST_CASE(single_type)
{
    typedef logging::attribute_set attr_set;
    typedef logging::attribute_value_set attr_values;
    typedef test_data< char > data;

    attrs::constant< int > attr1(10);
    attrs::constant< double > attr2(5.5);
    attrs::constant< std::string > attr3("Hello, world!");

    attr_set set1, set2, set3;
    set1[data::attr1()] = attr1;
    set1[data::attr2()] = attr2;

    attr_values values1(set1, set2, set3);
    values1.freeze();

    my_receiver recv;

    logging::value_visitor_invoker< int > invoker1;
    logging::value_visitor_invoker< double > invoker2;
    logging::value_visitor_invoker< std::string > invoker3;
    logging::value_visitor_invoker< char > invoker4;

    // These two extractors will find their values in the set
    recv.set_expected(10);
    BOOST_CHECK(invoker1(data::attr1(), values1, recv));

    recv.set_expected(5.5);
    BOOST_CHECK(invoker2(data::attr2(), values1, recv));

    // This one will not
    recv.set_expected();
    BOOST_CHECK(!invoker3(data::attr3(), values1, recv));

    // But it will find it in this set
    set1[data::attr3()] = attr3;

    attr_values values2(set1, set2, set3);
    values2.freeze();

    recv.set_expected("Hello, world!");
    BOOST_CHECK(invoker3(data::attr3(), values2, recv));

    // This one will find the sought attribute value, but it will have an incorrect type
    recv.set_expected();
    BOOST_CHECK(!invoker4(data::attr1(), values1, recv));

    // This one is the same, but there is a value of the requested type in the set
    BOOST_CHECK(!invoker1(data::attr2(), values1, recv));
}

// The test checks invokers specialized with type lists
BOOST_AUTO_TEST_CASE(multiple_types)
{
    typedef logging::attribute_set attr_set;
    typedef logging::attribute_value_set attr_values;
    typedef test_data< char > data;
    typedef mpl::vector< int, double, std::string, char >::type types;

    attrs::constant< int > attr1(10);
    attrs::constant< double > attr2(5.5);
    attrs::constant< std::string > attr3("Hello, world!");

    attr_set set1, set2, set3;
    set1[data::attr1()] = attr1;
    set1[data::attr2()] = attr2;

    attr_values values1(set1, set2, set3);
    values1.freeze();

    my_receiver recv;

    logging::value_visitor_invoker< types > invoker;

    // These two extractors will find their values in the set
    recv.set_expected(10);
    BOOST_CHECK(invoker(data::attr1(), values1, recv));

    recv.set_expected(5.5);
    BOOST_CHECK(invoker(data::attr2(), values1, recv));

    // This one will not
    recv.set_expected();
    BOOST_CHECK(!invoker(data::attr3(), values1, recv));

    // But it will find it in this set
    set1[data::attr3()] = attr3;

    attr_values values2(set1, set2, set3);
    values2.freeze();

    recv.set_expected("Hello, world!");
    BOOST_CHECK(invoker(data::attr3(), values2, recv));
}

// The test verifies the visit function
BOOST_AUTO_TEST_CASE(visit_function)
{
    typedef logging::attribute_set attr_set;
    typedef logging::attribute_value_set attr_values;
    typedef test_data< char > data;
    typedef mpl::vector< int, double, std::string, char >::type types;

    attrs::constant< int > attr1(10);
    attrs::constant< double > attr2(5.5);
    attrs::constant< std::string > attr3("Hello, world!");

    attr_set set1, set2, set3;
    set1[data::attr1()] = attr1;
    set1[data::attr2()] = attr2;

    attr_values values1(set1, set2, set3);
    values1.freeze();

    my_receiver recv;

    // These two extractors will find their values in the set
    recv.set_expected(10);
    BOOST_CHECK(logging::visit< types >(data::attr1(), values1, recv));

    recv.set_expected(5.5);
    BOOST_CHECK(logging::visit< double >(data::attr2(), values1, recv));

    // These will not
    recv.set_expected();
    BOOST_CHECK(!logging::visit< types >(data::attr3(), values1, recv));
    BOOST_CHECK(!logging::visit< char >(data::attr1(), values1, recv));

    // But it will find it in this set
    set1[data::attr3()] = attr3;

    attr_values values2(set1, set2, set3);
    values2.freeze();

    recv.set_expected("Hello, world!");
    BOOST_CHECK(logging::visit< std::string >(data::attr3(), values2, recv));
}


text:
//  Copyright (c) 2001 Daniel C. Nuffer
//  Copyright (c) 2001-2011 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(BOOST_SPIRIT_ITERATOR_FIXED_SIZE_QUEUE_MAR_16_2007_1137AM)
#define BOOST_SPIRIT_ITERATOR_FIXED_SIZE_QUEUE_MAR_16_2007_1137AM

#include <cstdlib>
#include <iterator>
#include <cstddef>

#include <boost/config.hpp>
#include <boost/assert.hpp>   // for BOOST_ASSERT
#include <boost/iterator_adaptors.hpp>

///////////////////////////////////////////////////////////////////////////////
//  Make sure we're using a decent version of the Boost.IteratorAdaptors lib
#if !defined(BOOST_ITERATOR_ADAPTORS_VERSION) || \
    BOOST_ITERATOR_ADAPTORS_VERSION < 0x0200
#error "Please use at least Boost V1.31.0 while compiling the fixed_size_queue class!"
#endif

///////////////////////////////////////////////////////////////////////////////
#define BOOST_SPIRIT_ASSERT_FSQ_SIZE                                          \
    BOOST_ASSERT(((m_tail + N + 1) - m_head) % (N+1) == m_size % (N+1))       \
    /**/

///////////////////////////////////////////////////////////////////////////////
namespace boost { namespace spirit { namespace detail
{
    ///////////////////////////////////////////////////////////////////////////
    template <typename Queue, typename T, typename Pointer>
    class fsq_iterator
    :   public boost::iterator_facade<
            fsq_iterator<Queue, T, Pointer>, T,
            std::random_access_iterator_tag
        >
    {
    public:
        typedef typename Queue::position_type position_type;
        typedef boost::iterator_facade<
                fsq_iterator<Queue, T, Pointer>, T,
                std::random_access_iterator_tag
            > base_type;

        fsq_iterator() {}
        fsq_iterator(position_type const &p_) : p(p_) {}

        position_type &get_position() { return p; }
        position_type const &get_position() const { return p; }

    private:
        friend class boost::iterator_core_access;

        typename base_type::reference dereference() const
        {
            return p.self->m_queue[p.pos];
        }

        void increment()
        {
            ++p.pos;
            if (p.pos == Queue::MAX_SIZE+1)
                p.pos = 0;
        }

        void decrement()
        {
            if (p.pos == 0)
                p.pos = Queue::MAX_SIZE;
            else
                --p.pos;
        }

        bool is_eof() const
        {
            return p.self == 0 || p.pos == p.self->size();
        }

        template <typename Q, typename T_, typename P>
        bool equal(fsq_iterator<Q, T_, P> const &x) const
        {
            if (is_eof())
                return x.is_eof();
            if (x.is_eof())
                return false;

            position_type const &rhs_pos = x.get_position();
            return (p.self == rhs_pos.self) && (p.pos == rhs_pos.pos);
        }

        template <typename Q, typename T_, typename P>
        typename base_type::difference_type distance_to(
            fsq_iterator<Q, T_, P> const &x) const
        {
            typedef typename base_type::difference_type difference_type;

            position_type const &p2 = x.get_position();
            std::size_t pos1 = p.pos;
            std::size_t pos2 = p2.pos;

            //  Undefined behavior if the iterators come from different
            //  containers
            BOOST_ASSERT(p.self == p2.self);

            if (pos1 < p.self->m_head)
                pos1 += Queue::MAX_SIZE;
            if (pos2 < p2.self->m_head)
                pos2 += Queue::MAX_SIZE;

            if (pos2 > pos1)
                return difference_type(pos2 - pos1);
            else
                return -difference_type(pos1 - pos2);
        }

        void advance(typename base_type::difference_type n)
        {
            //  Notice that we don't care values of n that can
            //  wrap around more than one time, since it would
            //  be undefined behavior anyway (going outside
            //  the begin/end range). Negative wrapping is a bit
            //  cumbersome because we don't want to case p.pos
            //  to signed.
            if (n < 0)
            {
                n = -n;
                if (p.pos < (std::size_t)n)
                    p.pos = Queue::MAX_SIZE+1 - (n - p.pos);
                else
                    p.pos -= n;
            }
            else
            {
                p.pos += n;
                if (p.pos >= Queue::MAX_SIZE+1)
                    p.pos -= Queue::MAX_SIZE+1;
            }
        }

    private:
        position_type p;
    };

    ///////////////////////////////////////////////////////////////////////////
    template <typename T, std::size_t N>
    class fixed_size_queue
    {
    private:
        struct position
        {
            fixed_size_queue* self;
            std::size_t pos;

            position() : self(0), pos(0) {}

            // The const_cast here is just to avoid to have two different
            //  position structures for the const and non-const case.
            // The const semantic is guaranteed by the iterator itself
            position(const fixed_size_queue* s, std::size_t p)
              : self(const_cast<fixed_size_queue*>(s)), pos(p)
            {}

            bool is_initialized() const { return self != 0; }
            void set_queue(fixed_size_queue* q) { self = q; }
        };

    public:
        // Declare the iterators
        typedef fsq_iterator<fixed_size_queue<T, N>, T, T*> iterator;
        typedef
            fsq_iterator<fixed_size_queue<T, N>, T const, T const*>
        const_iterator;
        typedef position position_type;

        friend class fsq_iterator<fixed_size_queue<T, N>, T, T*>;
        friend class fsq_iterator<fixed_size_queue<T, N>, T const, T const*>;

        fixed_size_queue();
        fixed_size_queue(const fixed_size_queue& x);
        fixed_size_queue& operator=(const fixed_size_queue& x);
        ~fixed_size_queue();

        void push_back(const T& e);
        void push_front(const T& e);
        void serve(T& e);
        void pop_front();

        bool empty() const
        {
            return m_size == 0;
        }

        bool full() const
        {
            return m_size == N;
        }

        iterator begin()
        {
            return iterator(position(this, m_head));
        }

        const_iterator begin() const
        {
            return const_iterator(position(this, m_head));
        }

        iterator end()
        {
            return iterator(position(0, m_tail));
        }

        const_iterator end() const
        {
            return const_iterator(position(0, m_tail));
        }

        std::size_t size() const
        {
            return m_size;
        }

        T& front()
        {
            return m_queue[m_head];
        }

        const T& front() const
        {
            return m_queue[m_head];
        }

    private:
        // Redefine the template parameters to avoid using partial template
        //  specialization on the iterator policy to extract N.
        BOOST_STATIC_CONSTANT(std::size_t, MAX_SIZE = N);

        std::size_t m_head;
        std::size_t m_tail;
        std::size_t m_size;
        T m_queue[N+1];
    };

    template <typename T, std::size_t N>
    inline
    fixed_size_queue<T, N>::fixed_size_queue()
        : m_head(0)
        , m_tail(0)
        , m_size(0)
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

    template <typename T, std::size_t N>
    inline
    fixed_size_queue<T, N>::fixed_size_queue(const fixed_size_queue& x)
        : m_head(x.m_head)
        , m_tail(x.m_tail)
        , m_size(x.m_size)
    {
        copy(x.begin(), x.end(), begin());
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

    template <typename T, std::size_t N>
    inline fixed_size_queue<T, N>&
    fixed_size_queue<T, N>::operator=(const fixed_size_queue& x)
    {
        if (this != &x)
        {
            m_head = x.m_head;
            m_tail = x.m_tail;
            m_size = x.m_size;
            copy(x.begin(), x.end(), begin());
        }
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);

        return *this;
    }

    template <typename T, std::size_t N>
    inline
    fixed_size_queue<T, N>::~fixed_size_queue()
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

    template <typename T, std::size_t N>
    inline void
    fixed_size_queue<T, N>::push_back(const T& e)
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);

        BOOST_ASSERT(!full());

        m_queue[m_tail] = e;
        ++m_size;
        ++m_tail;
        if (m_tail == N+1)
            m_tail = 0;

        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

    template <typename T, std::size_t N>
    inline void
    fixed_size_queue<T, N>::push_front(const T& e)
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);

        BOOST_ASSERT(!full());

        if (m_head == 0)
            m_head = N;
        else
            --m_head;

        m_queue[m_head] = e;
        ++m_size;

        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

    template <typename T, std::size_t N>
    inline void
    fixed_size_queue<T, N>::serve(T& e)
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);

        e = m_queue[m_head];
        pop_front();
    }

    template <typename T, std::size_t N>
    inline void
    fixed_size_queue<T, N>::pop_front()
    {
        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);

        ++m_head;
        if (m_head == N+1)
            m_head = 0;
        --m_size;

        BOOST_ASSERT(m_size <= N+1);
        BOOST_SPIRIT_ASSERT_FSQ_SIZE;
        BOOST_ASSERT(m_head <= N+1);
        BOOST_ASSERT(m_tail <= N+1);
    }

}}}

#undef BOOST_SPIRIT_ASSERT_FSQ_SIZE

#endif


text:
// Copyright (C) 2011-2020 Roki. Distributed under the MIT License
#ifndef INCLUDED_SROOK_ANY_HPP
#define INCLUDED_SROOK_ANY_HPP

#if __has_include(<any>)
#include <any>
#define POSSIBLE_TO_INCLUDE_STD_ANY 2017
#elif __has_include(<boost/any.hpp>)
#include <boost/any.hpp>
#define POSSIBLE_TO_INCLUDE_BOOST_ANY 2007
#elif (__cplusplus == 201703L)
#include <srook/cxx17/any.hpp>
#else

// C++03 code
#include <srook/config/nullptr.hpp>
#include <srook/memory/unique_ptr.hpp>
#include <srook/type_traits/enable_disable_if.hpp>
#include <stdexcept>
#include <typeinfo>

#ifdef POSSIBLE_TO_INCLUDE_STD_MEMORY
#define UNIQUE_PTR std::unique_ptr
#elif defined(POSSIBLE_TO_INCLUDE_BOOST_UNIQUE_PTR)
#define UNIQUE_PTR boost::unique_ptr
#else
#define UNIQUE_PTR srook::unique_ptr
#endif

#ifdef POSSIBLE_TO_USE_STD_NULLPTR
#define NULLPTR nullptr
#define NULLPTR_T std::nullptr_t
#else
#define NULLPTR srook::nullptr
#define NULLPTR_T srook::nullptr_t
#endif

namespace srook {
namespace adl_firewall {

class any {
    struct erasure_base {
	virtual erasure_base *clone() const = 0;
	virtual bool bool_tester() const throw() = 0;
	virtual const std::type_info &type() const = 0;
	virtual ~erasure_base() throw() {}
    };
    template <class T>
    struct erasure : erasure_base {
	erasure(const T &data) : data_(data) {}

	erasure_base *clone() const { return new erasure<T>(data_); }
	bool bool_tester() const throw() { return static_cast<bool>(data_); }
	const std::type_info &type() const { return typeid(T); }

	T &get() { return data_; }
	T &get() const { return data_; }

    private:
	T data_;
    };

    class Mover {
	Mover(any &a) : a(a) {}

    public:
	static Mover instance(any &a) throw() { return Mover(a); }
	any &a;
    };

public:
    explicit any() throw() : data_(NULLPTR) {}

    template <class T>
    any(const T &value) : data_(new erasure<T>(value))
    {
    }

    template <class T>
    any &operator=(const T &value)
    {
	data_.reset(new erasure<T>(value));
	return *this;
    }

    any(const any &other) : data_(other.data_->clone()) {}

    any(Mover other) : data_(other.a.data_.release()) {} // simulating move constructor

    any &operator=(const any &other)
    {
	data_.reset(other.data_->clone());
	return *this;
    }

    any &operator=(Mover other) // simulating move assign
    {
	data_.reset(other.a.data_.release());
	return *this;
    }

    operator bool() const
    {
	return data_->bool_tester();
    }

    template <class T>
    static T &get(any &this_)
    {
	return static_cast<erasure<T> *>(this_.data_.get())->get();
    }

    template <class T>
    static T &get(const any &this_)
    {
	return static_cast<erasure<T> *>(this_.data_.get())->get();
    }

    static Mover move(any &a) { return Mover::instance(a); }

    void reset()
    {
	data_.release();
    }

    bool has_value() const throw()
    {
	return bool(data_);
    }

    void swap(any &other) throw()
    {
	std::swap(other.data_, data_);
    }

    template <class T>
    operator T()
    {
	return get<T>(*this);
    }

private:
    UNIQUE_PTR<erasure_base> data_;
};

template <class T>
T operator+(const T &value, const any &a)
{
    return value + adl_firewall::any::get<T>(a);
}
template <class T>
T operator+(const any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) + value;
}
template <class T>
T &operator+=(T &value, const any &a)
{
    return value += adl_firewall::any::get<T>(a) + value;
}
template <class T>
T &operator+=(any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) += value;
}

template <class T>
T operator-(const T &value, const any &a)
{
    return value + adl_firewall::any::get<T>(a);
}
template <class T>
T operator-(const any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) - value;
}
template <class T>
T &operator-=(T &value, const any &a)
{
    return value -= adl_firewall::any::get<T>(a);
}
template <class T>
T &operator-=(any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) -= value;
}

template <class T>
T operator*(const T &value, const any &a)
{
    return value * adl_firewall::any::get<T>(a);
}
template <class T>
T operator*(const any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) * value;
}
template <class T>
T &operator*=(T &value, const any &a)
{
    return value *= adl_firewall::any::get<T>(a);
}
template <class T>
T &operator*=(any &a, const T &value)
{
    return adl_firewall::any::get<T>(a) *= value;
}

template <class T>
T operator/(const T &value, const any &a) throw(std::range_error)
{
    if (!adl_firewall::any::get<T>(a)) throw std::range_error("Divided by zero.");
    return value / adl_firewall::any::get<T>(a);
}
template <class T>
T operator/(const any &a, const T &value) throw(std::range_error)
{
    if (!value) throw std::range_error("Divided by zero.");
    return adl_firewall::any::get<T>(a) / value;
}
template <class T>
T &operator/=(T &value, const any &a) throw(std::range_error)
{
    if (!adl_firewall::any::get<T>(a)) throw std::range_error("Divided by zero.");
    return value / adl_firewall::any::get<T>(a);
}
template <class T>
T &operator/=(any &a, const T &value) throw(std::range_error)
{
    if (!value) throw std::range_error("Divided by zero.");
    return adl_firewall::any::get<T>(a) /= value;
}
} // namespace adl_firewall

using adl_firewall::any;

} // namespace srook

#undef UNIQUE_PTR
#endif
#endif

max_stars_repo_name: falgon/srookCppLibraries

max_issues_repo_name: falgon/srookCppLibraries

max_forks_repo_name: falgon/srookCppLibraries


text:
#include "Object.h"
#include "parameters.h"
#include <boost/lexical_cast.hpp>

#include <Eigen/Geometry>
#include <tf/transform_datatypes.h>

namespace hector_object_tracker {

std::map<std::string,unsigned int> Object::object_count;
std::string Object::object_namespace;

Object::Object(const std::string class_id, const std::string object_id)
{
  if (!class_id.empty()) {
    this->info.class_id = class_id;
  } else
  {
    this->info.class_id = "object";
  }

  if (!object_id.empty()) {
    this->info.object_id = object_id;
  } else {
    this->info.object_id = this->info.class_id + "_" + boost::lexical_cast<std::string>(object_count[this->info.class_id]++);
  }

  // if (this->info.object_id[0] != '/') this->info.object_id = object_namespace + "/" + this->info.object_id;
}

Object::Object(const hector_worldmodel_msgs::Object& other) {
  *this = other;
}

Object::~Object()
{
  ROS_WARN("who kill me?");
}

void Object::reset() {
  object_count.clear();
}

void Object::getMessage(hector_worldmodel_msgs::Object& object) const {
  object.header = this->header;
  this->getPoseWithCovariance(object.pose);
  object.info   = this->info;
  object.state  = this->state;
}

hector_worldmodel_msgs::Object Object::getMessage() const {
  hector_worldmodel_msgs::Object object;
  getMessage(object);
  return object;
}

void Object::getPoseWithCovariance(geometry_msgs::PoseWithCovariance& pose) const {
  getPose(pose.pose);
  getCovariance(pose.covariance);
}

void Object::setPose(const geometry_msgs::PoseWithCovariance& pose) {
  setPose(pose.pose);
  setCovariance(pose.covariance);
}

void Object::getPose(geometry_msgs::Pose& pose) const
{
  pose.position.x = this->position.x();
  pose.position.y = this->position.y();
  pose.position.z = this->position.z();
  pose.orientation.w = this->orientation.w();
  pose.orientation.x = this->orientation.x();
  pose.orientation.y = this->orientation.y();
  pose.orientation.z = this->orientation.z();
}

void Object::getPose(tf::Pose& pose) const
{
  pose.getOrigin().setValue(this->position.x(), this->position.y(), this->position.z());
  pose.getBasis().setRotation(tf::Quaternion(this->orientation.x(), this->orientation.y(), this->orientation.z(), this->orientation.w()));
}

void Object::setPose(const geometry_msgs::Pose& pose)
{
  setPosition(pose.position);
  setOrientation(pose.orientation);
}

void Object::setPose(const tf::Pose &pose)
{
  setPosition(pose.getOrigin());
  tf::Quaternion rot;
  pose.getBasis().getRotation(rot);
  setOrientation(rot);
}

const Eigen::Vector3f& Object::getPosition() const {
  return position;
}

void Object::setPosition(const Eigen::Vector3f& position) {
  this->position = position;
}

void Object::setPosition(const geometry_msgs::Point& position) {
  this->position << position.x, position.y, position.z;
}

void Object::setPosition(const tf::Point& position)
{
  this->position << position.x(), position.y(), position.z();
}

const Eigen::Quaternionf& Object::getOrientation() const
{
  return this->orientation;
}

void Object::setOrientation(const geometry_msgs::Quaternion& orientation)
{
  this->orientation.coeffs() << orientation.x, orientation.y, orientation.z, orientation.w;
}

void Object::setOrientation(const tf::Quaternion& orientation)
{
  this->orientation.coeffs() << orientation.x(), orientation.y(), orientation.z(), orientation.w();
}

const Eigen::Matrix3f& Object::getCovariance() const {
  return covariance;
}

void Object::getCovariance(geometry_msgs::PoseWithCovariance::_covariance_type& covariance) const {
  covariance[0]  = this->covariance(0,0);
  covariance[1]  = this->covariance(0,1);
  covariance[2]  = this->covariance(0,2);
  covariance[6]  = this->covariance(1,0);
  covariance[7]  = this->covariance(1,1);
  covariance[8]  = this->covariance(1,2);
  covariance[12] = this->covariance(2,0);
  covariance[13] = this->covariance(2,1);
  covariance[14] = this->covariance(2,2);
}

void Object::setCovariance(const Eigen::Matrix3f& eigen) {
  this->covariance = eigen;
}

void Object::setCovariance(const tf::Matrix3x3& tf) {
  this->covariance << tf[0][0], tf[0][1], tf[0][2],
                      tf[1][0], tf[1][1], tf[1][2],
                      tf[2][0], tf[2][1], tf[2][2];
}

void Object::setCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& covariance) {
  this->covariance << covariance[0],  covariance[1],  covariance[2],
                      covariance[6],  covariance[7],  covariance[8],
                      covariance[12], covariance[13], covariance[14];
}

void Object::setState(const StateType& state) {
  if (this->state.state == state) return;
  ROS_INFO("Setting object state for %s to %s", getObjectId().c_str(), hector_worldmodel_msgs::getObjectStateString(state));
  this->state.state = state;
}

void Object::intersect(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
  Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
  tf::Quaternion orientationB = poseB.getRotation();
    // old cov/covariance is A , new cov/covIn is B
  float omega = 0.5f;

  Eigen::Matrix3f A(covariance.inverse() * omega);
  Eigen::Matrix3f B(covarianceB.inverse() * (1.0f - omega));
  double infA = 1./covariance.trace();
  double infB = 1./covarianceB.trace();

  covariance = (A + B).inverse();
  position = covariance * (A * position + B * positionB);
  updateOrientation(orientationB, infB / (infA + infB));

  setPosition(position);
  setCovariance(covariance);
  addSupport(support);
}

void Object::update(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
  Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
  tf::Quaternion orientationB = poseB.getRotation();
    // old information is A , new information is B

  Eigen::Matrix3f A(covariance.inverse());
  Eigen::Matrix3f B(covarianceB.inverse());
  double infA = 1./covariance.trace();
  double infB = 1./covarianceB.trace();

  covariance = (A + B).inverse();
  position = covariance * (A * position + B * positionB);
  updateOrientation(orientationB, infB / (infA + infB));

  setPosition(position);
  setCovariance(covariance);
  addSupport(support);
}

void Object::updateOrientation(const tf::Quaternion& orientationB, double slerp_factor) {
  // update orientation of objects using low-pass filtering
  if (parameter(_with_orientation, getClassId())) {
      tf::Quaternion q(orientation.x(), orientation.y(), orientation.z(), orientation.w());
      q.slerp(orientationB, slerp_factor);
      setOrientation(q);
  }
  // or simply set new orientation
  else {
    setOrientation(orientationB);
  }
}

void Object::getVisualization(visualization_msgs::MarkerArray &markers) const {
  visualization_msgs::Marker marker;
  std::string postfix;

  // default color
  marker.color = parameter(_marker_color, this->info.class_id);

  switch(this->state.state) {
    case hector_worldmodel_msgs::ObjectState::CONFIRMED:
      marker.color.r = 0.0;
      marker.color.g = 0.8;
      marker.color.b = 0.0;
      postfix = " (CONFIRMED)";
      break;
    case hector_worldmodel_msgs::ObjectState::DISCARDED:
      marker.color.a = 0.5;
      postfix = " (DISCARDED)";
      break;
    case hector_worldmodel_msgs::ObjectState::INACTIVE:
      marker.color.a = 0.5;
      postfix = " (INACTIVE)";
      break;
    case hector_worldmodel_msgs::ObjectState::UNKNOWN:
      marker.color.a = 0.5;
      break;
    case hector_worldmodel_msgs::ObjectState::PENDING:
      marker.color.a = 0.5;
      postfix = " (PENDING)";
      break;
    default:
      break;
  }

  marker.header = this->header;
  marker.action = visualization_msgs::Marker::ADD;
  getPose(marker.pose);
  marker.ns = this->info.class_id;

  marker.type = visualization_msgs::Marker::ARROW;
  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 1.0;
  // markers.markers.push_back(marker);

  marker.type = visualization_msgs::Marker::SPHERE;
  marker.scale.x = 0.1;
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
  markers.markers.push_back(marker);

  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
  marker.text = (!this->info.name.empty() ? this->info.name : this->info.object_id) + postfix;
  marker.scale.x = 0.0;
  marker.scale.y = 0.0;
  marker.scale.z = 0.1;
  marker.pose.position.z += 1.5 * marker.scale.z;
  markers.markers.push_back(marker);
}

void Object::setNamespace(const std::string &ns) {
  object_namespace = ns;
}

Object& Object::operator =(const hector_worldmodel_msgs::Object& other) {
  header = other.header;
  info = other.info;
  state = other.state;
  setPose(other.pose);
  return *this;
}

ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame) const
{
  return transform(tf, target_frame, this->header.stamp);
}

ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame, const ros::Time& target_time) const
{
  tf::StampedTransform transform;
  tf.lookupTransform(target_frame, this->header.frame_id, target_time, transform);

  ObjectPtr result(new Object(*this));

  tf::Pose pose;
  this->getPose(pose);

  // transform pose
  pose = transform * pose;
  result->setPose(pose);

  // rotate covariance matrix
  tf::Matrix3x3 rotation(transform.getBasis());
  tf::Matrix3x3 cov(covariance(0,0), covariance(0,1), covariance(0,2),
                    covariance(1,0), covariance(1,1), covariance(1,2),
                    covariance(2,0), covariance(2,1), covariance(2,2));
  result->setCovariance(rotation * cov * rotation.transpose());

  // set new frame_id
  result->header.frame_id = target_frame;

  return result;
}

double Object::getDistance(const Object &other)
{
  return (this->getPosition() - other.getPosition()).norm();
}

} // namespace hector_object_tracker