text:
/* command_template.cc
   Jeremy Barnes, 27 August 2013
   Copyright (c) 2013 mldb.ai inc.  All rights reserved.
   This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

   Template to allow command lines to be created by subsituting values.
*/

#include "command_expression.h"
#include "command_expression_impl.h"
#include "mldb/base/parse_context.h"
#include "mldb/base/exc_assert.h"
#include "mldb/utils/csv.h"
#include <mutex>
#include <thread>
#include "mldb/types/periodic_utils.h"
#include "mldb/types/structure_description.h"
#include "mldb/types/vector_description.h"
#include "mldb/utils/json_utils.h"
#include <boost/lexical_cast.hpp>
#include <boost/range/adaptor/reversed.hpp>

using namespace std;

namespace MLDB {

namespace PluginCommand {

namespace {

Json::Value dateFormat(const std::vector<Json::Value> & params)
{
    Date date = jsonDecode<Date>(params[1]);

    //cerr << "params[0] = " << params[0] << endl;
    //cerr << "params[0].type() = " << params[0].type() << endl;

    std::string format = params.at(0).asString();
    return filenameFor(date, format);
}

Json::Value dateFormatIso8601(const std::vector<Json::Value> & params)
{
    Date date = jsonDecode(params.at(0), (Date *)0);
    return date.printIso8601();
}

Json::Value join(const std::vector<Json::Value> & params)
{
    string result;
    const Json::Value & arr = params.at(0);
    string separator = params.at(1).asString();

    for (unsigned i = 0;  i < arr.size();  ++i) {
        if (i != 0)
            result += separator;
        result += stringRender(arr[i]);
    }

    return result;
}

std::vector<Date> iterDates(Date startDate, Date endDate,
                            TimePeriod step)
{
    if (endDate < startDate)
        throw MLDB::Exception("end date less than start date");
    if (step.number <= 0)
        throw MLDB::Exception("time interval goes the wrong way");

    vector<Date> result;
    for (Date d = startDate;  d < endDate;  d += step) {
        if (result.size() > 100000)
            throw MLDB::Exception("list of dates is too long");
        result.push_back(d);
    }

    return result;
}

Date dateAdd(Date input, TimePeriod period)
{
    return input + period;
}

Json::Value splitPath(const std::vector<Json::Value> & params)
{
    string name = params[0].asString();

    Json::Value result;
    auto pos = name.find("://");
    int start = 0;
    if (pos != string::npos) {
        result["scheme"] = string(name, 0, pos);
        start = pos + 3;

        pos = name.find('/', start);
        if (pos == string::npos) {
            result["entity"] = "";
        }
        else {
            result["entity"] = string(name, start, pos - start);
            start = pos;
        }
    }
    else {
        result["scheme"] = "";
        result["entity"] = "";
        start = 0;
    }

    string filename(name, start);
    result["dirpath"] = filename;

    string file;

    auto lastSlash = filename.rfind('/');
    if (lastSlash == string::npos) {
        result["directory"] = "";
        result["file"] = file = filename;
    }
    else {
        result["directory"] = string(filename, 0, lastSlash);
        result["file"] = file = string(filename, lastSlash + 1);
    }

    auto firstPeriod = file.find('.');
    if (firstPeriod == string::npos) {
        result["extension"] = "";
        result["basename"] = file;
    }
    else {
        result["extension"] = string(file, firstPeriod + 1);
        result["basename"] = string(file, 0, firstPeriod);
    }

    //cerr << "parsed " << name << " into " << result << endl;

    return result;
}

Json::Value basename(const std::vector<Json::Value> & params)
{
    return splitPath(params)["basename"];
}

Json::Value csv(const std::vector<Json::Value> & params)
{
    std::string result;
    for (unsigned i = 0;  i < params.size();  ++i) {
        if (i != 0)
            result += ',';
        if (params[i].isArray()) {
            for (unsigned j = 0;  j < params[i].size();  ++j) {
                if (j != 0)
                    result += ',';
                result += csv_escape(stringRender(params[i][j]));
            }
        }
        else result += csv_escape(stringRender(params[i]));
    }
    return result;
}

Json::Value tsv(const std::vector<Json::Value> & params)
{
    std::string result;
    for (unsigned i = 0;  i < params.size();  ++i) {
        if (i != 0)
            result += '\t';
        if (params[i].isArray()) {
            for (unsigned j = 0;  j < params[i].size();  ++j) {
                if (j != 0)
                    result += '\t';
                result += stringRender(params[i][j]);
            }
        }
        else result += stringRender(params[i]);
    }
    return result;
}

Json::Value hash(const std::vector<Json::Value> & params)
{
    if (params.size() != 1)
        throw MLDB::Exception("hash() function takes exactly one argument");
    return jsonHash(params[0]);
}

} // file scope

/*****************************************************************************/
/* COMMAND EXPRESSION VARIABLES                                              */
/*****************************************************************************/

const CommandExpressionVariables *
CommandExpressionVariables::
builtins()
{
    static CommandExpressionVariables * value = 0;
    static std::mutex lock;

    if (value)
        return value;
    std::unique_lock<std::mutex> guard(lock);
    if (value)
        return value;

    value = new CommandExpressionVariables(nullptr);
    value->addFunction("dateFormat", dateFormat);
    value->addFunction("dateFormatIso8601", dateFormatIso8601);
    value->addFunction("join", join);
    value->addFunction("splitPath", splitPath);
    value->addFunction("basename", basename);
    value->addFunction("csv", csv);
    value->addFunction("tsv", tsv);
    value->addBoundFunction("iterDates", iterDates);
    value->addBoundFunction("dateAdd", dateAdd);
    value->addFunction("min", jsonMinVector);
    value->addFunction("max", jsonMaxVector);
    value->addFunction("flatten", flatten);
    value->addFunction("hash", hash);

    return value;
}

Json::Value
CommandExpressionVariables::
applyFunction(const std::string & functionName,
              const std::vector<Json::Value> & functionArgs) const
{
    //using namespace std;
    //cerr << functionName << "(" << functionArgs << ")" << endl;

    auto it = functions.find(functionName);
    if (it == functions.end()) {
        if (outer)
            return outer->applyFunction(functionName, functionArgs);
        else {
            using namespace std;
            cerr << "function " << functionName << " is not registered"
                 << endl;
            throw MLDB::Exception("function " + functionName + " is not registered");
        }
    }
    try {
        return it->second(functionArgs);
    } catch (const std::exception & exc) {
        throw MLDB::Exception("error trying to apply %s to args %s: %s",
                            functionName.c_str(),
                            boost::lexical_cast<std::string>(functionArgs).c_str(),
                            exc.what());
    }
}

/*****************************************************************************/
/* COMMAND EXPRESSION                                                        */
/*****************************************************************************/

std::shared_ptr<CommandExpression>
CommandExpression::
parse(const std::string & val)
{
    ParseContext context(val, val.c_str(), val.c_str() + val.length());

    std::shared_ptr<CommandExpression> res;
    if (context.match_literal("%!"))
        res = parseArgumentExpression(context);
    else res = parseExpression(context, false /* stopOnWhitespace */);

    context.expect_eof();

    return res;
}

std::shared_ptr<CommandExpression>
CommandExpression::
parseArgumentExpression(const std::string & val)
{
    ParseContext context(val, val.c_str(), val.c_str() + val.length());

    auto res = parseArgumentExpression(context);

    context.expect_eof();

    return res;
}

std::shared_ptr<CommandExpression>
CommandExpression::
parse(const std::vector<std::string> & vals)
{
    std::shared_ptr<ArrayExpression> expr(new ArrayExpression());

    for (auto & s: vals)
        expr->clauses.push_back(parse(s));

    return expr;
}

std::shared_ptr<CommandExpression>
CommandExpression::
parseExpression(ParseContext & context, bool stopOnWhitespace)
{
    // Default is concat... we stop when we have a percent

    std::string current;

    std::shared_ptr<ConcatExpression> expr(new ConcatExpression());

    ParseContext::Hold_Token token(context);

    auto addContext = [&] (std::shared_ptr<CommandExpression> expr)
        {
            expr->surfaceForm = token.captured();
            return expr;
        };

    auto flush = [&] ()
        {
            if (!current.empty()) {
                expr->clauses.push_back(std::make_shared<LiteralExpression>(current));
                current = "";
            }
        };

    while (context) {

        if (stopOnWhitespace) {
            while (context && *context != '%' && !isspace(*context))
                current += *context++;
            if (!context || isspace(*context))
                break;
        }
        else {
            while (context && *context != '%')
                current += *context++;
            if (!context) break;
        }

        *context++;

        if (!context || isspace(*context)) {
            current += "%";
            continue;
        }

        if (context.match_literal('{')) {
            flush();

            expr->clauses.push_back(parseArgumentExpression(context));

            context.expect_literal('}');
        }
        else {
            // %% is a percent sign
            if (context.match_literal("%")) {
                current += "%";
                continue;
            }
            flush();
            // % something is the variable of that name
            expr->clauses.push_back(std::make_shared<VariableExpression>(string(1, *context++)));
        }
    }

    flush();

    return addContext(expr);
}

std::shared_ptr<CommandExpression>
CommandExpression::
parseArgumentExpression(ParseContext & context)
{
    ParseContext::Hold_Token token(context);

    auto addContext = [&] (std::shared_ptr<CommandExpression> expr)
        {
            expr->surfaceForm = token.captured();
            return expr;
        };

    context.skip_whitespace();

    std::shared_ptr<CommandExpression> result;

    if (context.match_literal('\'')) {
        // string literal
        string value;
        while (*context != '\'')
            value.push_back(*context++);
        context.expect_literal('\'');
        result = std::make_shared<LiteralExpression>(value);
    }
    else if (*context == '\"') {
        // string literal
        std::string value = expectJsonStringAscii(context);
        result = std::make_shared<LiteralExpression>(value);
    }
    else if (context.match_literal('[')) {
        // Inline array expression
        //cerr << "inline array" << endl;
        std::vector<std::shared_ptr<CommandExpression> > elements;
        context.skip_whitespace();
        if (!context.match_literal(']')) {
            while (*context != ']') {
                context.skip_whitespace();
                elements.push_back(parseArgumentExpression(context));
                context.skip_whitespace();
                if (!context.match_literal(','))
                    break;
            }

            context.expect_literal(']', "expected array close");
        }
        result = std::make_shared<InlineArrayExpression>(elements);
    }
    else if (context.match_literal('{')) {
        // Inline object expression
        std::vector<std::pair<std::shared_ptr<CommandExpression>,
                              std::shared_ptr<CommandExpression> > > elements;
        context.skip_whitespace();
        if (!context.match_literal('}')) {
            while (*context != '}') {
                context.skip_whitespace();
                auto kexpr = parseArgumentExpression(context);
                context.expect_literal(':', "expected colon after object key");
                context.skip_whitespace();
                auto vexpr = parseArgumentExpression(context);
                context.skip_whitespace();

                elements.push_back(make_pair(kexpr, vexpr));

                if (!context.match_literal(','))
                    break;
            }

            context.expect_literal('}', "expected object close");
        }
        result = std::make_shared<ObjectExpression>(elements);
    }
    else if (context.match_literal('%')) {
        context.exception("nested expression literals not yet supported");
    }
    else if (context.match_literal("map ")) {
        //cerr << "map expression" << endl;
        context.skip_whitespace();

        // Extract entity names

        std::vector<MapExpression::IterExpression> expressions;

        for (;;) {
            string entityName;

            while (isalnum(*context) || *context == '_')
                entityName += *context++;
            context.skip_whitespace();
            context.expect_literal(':', ("expected colon after map variable name " + entityName).c_str());
            context.skip_whitespace();

            auto iterExpr = parseArgumentExpression(context);

            expressions.push_back({entityName, iterExpr});

            context.skip_whitespace();
            if (context.match_literal(',')) {
                context.skip_whitespace();
                continue;
            }
            context.expect_literal("->", "expected arrow after map iter expr");
            break;
        }

        context.skip_whitespace();
        auto applyExpr = parseArgumentExpression(context);

        result = std::make_shared<MapExpression>(expressions, applyExpr);
    }
    else if (isalpha(*context) || *context == '_') {
        // Must be a variable or argument name

        string entityName;
        bool isFunction = false;
        vector<shared_ptr<CommandExpression> > args;

        while (isalnum(*context) || *context == '_')
            entityName += *context++;

        if (*context == ' ' && false) {
            context.skip_whitespace();
            // Expression with space separated args
            isFunction = true;
            args.push_back(parseArgumentExpression(context));
            context.skip_whitespace();
            if (!context.match_literal(',')) {
                context.expect_literal(')');
            }
        }
        else if (context.match_literal('(')) {
            // Expression with arguments in paranthesis

            isFunction = true;

            while (!context.match_literal(')')) {
                args.push_back(parseArgumentExpression(context));
                context.skip_whitespace();
                if (!context.match_literal(',')) {
                    context.expect_literal(')');
                    break;
                }
            }
        }

        if (isFunction)
            result = std::make_shared<FunctionExpression>(entityName, args);
        else
            result = std::make_shared<VariableExpression>(entityName);

    }
    else result = addContext(std::make_shared<JsonLiteralExpression>(expectJson(context)));

    // Now look for operators that modify the output of the previous
    while (true) {
        context.skip_whitespace();

        if (context.match_literal('.')) {
            string fieldName;
            while (isalnum(*context) || *context == '_')
                fieldName += *context++;

            result = std::make_shared<ExtractFieldExpression>(fieldName, result);
        }
        else if (context.match_literal('[')) {
            auto element = parseArgumentExpression(context);
            result = std::make_shared<ExtractElementExpression>(element, result);
            context.skip_whitespace();
            context.expect_literal(']');
        }
        else if (context.match_literal('+')) {
            auto rhs = parseArgumentExpression(context);
            result = std::make_shared<PlusExpression>(result, rhs);
        }
        else if (context.match_literal('/')) {
            auto rhs = parseArgumentExpression(context);
            result = std::make_shared<DivideExpression>(result, rhs);
        }
        else break;
    }

    return addContext(result);
}

std::string
argumentRender(const Json::Value & val)
{
    using std::to_string;

    switch (val.type()) {
    case Json::nullValue:    return "";
    case Json::intValue:     return to_string(val.asInt());
    case Json::uintValue:    return to_string(val.asUInt());
    case Json::realValue:    return to_string(val.asDouble());
    case Json::booleanValue: return to_string(val.asBool());
    case Json::stringValue:  return val.asString();
    case Json::arrayValue: {
        std::string str;
        for (auto & v: val) {
            if (!str.empty())
                str += " ";
            str += shellEscape(argumentRender(v));
        }
        return str;
    }
    default:
        throw MLDB::Exception("can't render value " + val.toString() + " as string");
    }
}

std::vector<std::string>
commandRender(const Json::Value & val)
{
    //cerr << "commandRender " << val << endl;

    using std::to_string;

    switch (val.type()) {
    case Json::nullValue:    return { "" };
    case Json::intValue:     return { to_string(val.asInt()) };
    case Json::uintValue:    return { to_string(val.asUInt()) };
    case Json::realValue:    return { to_string(val.asDouble()) };
    case Json::booleanValue: return { to_string(val.asBool()) };
    case Json::stringValue:  return { val.asString() };
    case Json::arrayValue: {
        std::vector<std::string> str;
        for (auto & v: val) {
            str.push_back(argumentRender(v));
        }
        return str;
    }
    default:
        throw MLDB::Exception("can't render value " + val.toString() + " as command");
    }
}

std::string
stringRender(const Json::Value & val)
{
    using std::to_string;

    switch (val.type()) {
    case Json::nullValue:    return "";
    case Json::intValue:     return to_string(val.asInt());
    case Json::uintValue:    return to_string(val.asUInt());
    case Json::realValue:    return to_string(val.asDouble());
    case Json::booleanValue: return to_string(val.asBool());
    case Json::stringValue:  return val.asString();
    case Json::arrayValue: {
        std::string str;
        for (auto & v: val) {
            if (!str.empty())
                str += " ";
            str += stringRender(v);
        }
        return str;
    }
    default:
        throw MLDB::Exception("can't render value " + val.toString() + " as string");
    }
}

/*****************************************************************************/
/* COMMAND TEMPLATE                                                          */
/*****************************************************************************/

void
StringTemplate::
parse(const std::string & command)
{
    ParseContext context(command,
                              command.c_str(), command.c_str() + command.size());

    expr = CommandExpression::parseExpression(context, false /* stop on whitespace */);

    context.expect_eof();
}

std::string
StringTemplate::
operator () (CommandExpressionContext & context) const
{
    return stringRender(expr->apply(context));
}

std::string
StringTemplate::
operator () (const std::initializer_list<std::pair<std::string, std::string> > & vals) const
{
    CommandExpressionContext context(vals);
    return operator () (context);
}

/*****************************************************************************/
/* COMMAND TEMPLATE                                                          */
/*****************************************************************************/

void
CommandTemplate::
parse(const std::string & command)
{
    ParseContext context(command,
                              command.c_str(), command.c_str() + command.size());

    while (context) {
        commandLine.push_back
            (CommandExpression::parseExpression
             (context, true /* stop on whitespace */));
        context.skip_whitespace();
    }
}

void
CommandTemplate::
parse(const std::vector<std::string> & cmdline)
{
    for (unsigned i = 0;  i < cmdline.size();  ++i)
        commandLine.push_back(CommandExpression::parse(cmdline[i]));
}

Command
CommandTemplate::
operator () (CommandExpressionContext & context) const
{
    Command result;

    for (auto & c: commandLine) {
        Json::Value r = c->apply(context);
        //cerr << "result of " << c->surfaceForm << " is " << r << endl;
        vector<string> vals = commandRender(r);
        result.cmdLine.insert(result.cmdLine.end(), vals.begin(), vals.end());
    }

    return result;
}

Command
CommandTemplate::
operator () (const std::initializer_list<std::pair<std::string, std::string> > & vals) const
{
    CommandExpressionContext context(vals);
    return operator () (context);
}

#if 0
struct CommandTemplateDescription
    : public StructureDescriptionImpl<CommandTemplate, CommandTemplateDescription> {

    CommandTemplateDescription();
};

inline CommandTemplateDescription * getDefaultDescription(CommandTemplate*)
{
    return new CommandTemplateDescription();
}
#endif

DEFINE_STRUCTURE_DESCRIPTION(CommandTemplate);

CommandTemplateDescription::
CommandTemplateDescription()
{
    //addField("env", &CommandTemplate::env, "expression to generate the environment");
    addField("commandLine", &CommandTemplate::commandLine, "expression to generate the command line");
}

DEFINE_VALUE_DESCRIPTION_NS(std::shared_ptr<CommandExpression>, CommandExpressionDescription);
DEFINE_VALUE_DESCRIPTION_NS(StringTemplate, StringTemplateDescription);

} // namespace PluginCommand
} // namespace MLDB

max_stars_repo_name: kstepanmpmg/mldb

max_issues_repo_name: tomzhang/mldb

max_forks_repo_name: matebestek/mldb


text:
#include "ScanMatch.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace lidar_slam {

using std::sqrt;
using std::fabs;
using std::asin;
using std::atan2;
using std::pow;

ScanMatch::ScanMatch(const size_t maxIterations)
    : _maxIterations(maxIterations), _deltaTAbort(0.05), _deltaRAbort(0.05),
      _useScore(true), _match_count(0), _fail_match_count(0), _total_score(0),
      _score_threshold(800), _match_percentage_threshold(0.4),
      _referenceCornerCloudDS(new CloudI()),
      _referenceSurfCloudDS(new CloudI()), _CornerCloudDS(new CloudI()),
      _SurfCloudDS(new CloudI()) {

  _downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
  _downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);

  _fineScore = false;
}

ScanMatch::~ScanMatch() {
  std::cout << "[ScanMatch]\n"
            << " ,match_count:" << _match_count
            << " ,fail_match_count:" << _fail_match_count
            << " ,averageScore:" << getAverageScore() << std::endl;
}

double ScanMatch::getScore(const CloudI &coeffCloud) {
  double score = 0;
  for (int i = 0; i < coeffCloud.points.size(); ++i) {
    const PointI &p = coeffCloud.points[i];
    score += std::exp(-fabs(p.intensity));
  }
  return score;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Twist &transformf) {

  if (referenceCornerCloud->points.size() < 50 ||
      referenceSurfCloud->points.size() < 100) {
    std::cout << "reference cloud points too few." << std::endl;
    return false;
  }
  Twist transform = transformf;

  PointI pointSel, pointOri, pointProj, coeff;
  std::vector<int> pointSearchInd(5, 0);
  std::vector<float> pointSearchSqDis(5, 0);

    nanoflann::KdTreeFLANN<PointI> kdtreeCorner;
    nanoflann::KdTreeFLANN<PointI> kdtreeSurf;
      /*
  pcl::KdTreeFLANN<PointI> kdtreeCorner;
  pcl::KdTreeFLANN<PointI> kdtreeSurf;
  */

  kdtreeCorner.setInputCloud(referenceCornerCloud);
  kdtreeSurf.setInputCloud(referenceSurfCloud);

  bool converge = false;
  bool isDegenerate = false;
  Eigen::Matrix<float, 6, 6> matP;

  size_t CornerNum = CornerCloud->points.size();
  size_t SurfNum = SurfCloud->points.size();

  pcl::PointCloud<PointI> laserCloudOri;
  pcl::PointCloud<PointI> coeffSel;

  int line_match_count = 0;
  int plane_match_count = 0;
  size_t iterCount;
  for (iterCount = 0; iterCount < _maxIterations; iterCount++) {
    laserCloudOri.clear();
    coeffSel.clear();
    line_match_count = 0;
    plane_match_count = 0;

    for (int i = 0; i < CornerNum; i++) {
      pointOri = CornerCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        const Eigen::Vector3f &point = pointSel.getVector3fMap();
        Eigen::Vector3f lineA, lineB;
        if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
          PointI coefficients;
          if (getCornerFeatureCoefficients(lineA, lineB, point, coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          line_match_count++;
        }
      }
    }

    for (int i = 0; i < SurfNum; i++) {
      pointOri = SurfCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        Eigen::Vector4f planeCoef;
        if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
          PointI coefficients;
          if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                            coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          plane_match_count++;
        }
      }
    }

    float srx = transform.rot_x.sin();
    float crx = transform.rot_x.cos();
    float sry = transform.rot_y.sin();
    float cry = transform.rot_y.cos();
    float srz = transform.rot_z.sin();
    float crz = transform.rot_z.cos();

    size_t laserCloudSelNum = laserCloudOri.points.size();
    if (laserCloudSelNum < 50) {
      ROS_WARN("matched cloud points too few. Matched/Input:  %d / %d", laserCloudSelNum, CornerNum+SurfNum);
      break;
    }

    Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
    Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
    Eigen::Matrix<float, 6, 6> matAtA;
    Eigen::VectorXf matB(laserCloudSelNum);
    Eigen::VectorXf matAtB;
    Eigen::VectorXf matX;

    for (int i = 0; i < laserCloudSelNum; i++) {
      pointOri = laserCloudOri.points[i];
      coeff = coeffSel.points[i];
      /*
      float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y -
                   srx * sry * pointOri.z) *
                      coeff.x +
                  (-srx * srz * pointOri.x - crz * srx * pointOri.y -
                   crx * pointOri.z) *
                      coeff.y +
                  (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y -
                   cry * srx * pointOri.z) *
                      coeff.z;

      float ary = ((cry * srx * srz - crz * sry) * pointOri.x +
                   (sry * srz + cry * crz * srx) * pointOri.y +
                   crx * cry * pointOri.z) *
                      coeff.x +
                  ((-cry * crz - srx * sry * srz) * pointOri.x +
                   (cry * srz - crz * srx * sry) * pointOri.y -
                   crx * sry * pointOri.z) *
                      coeff.z;

      float arz = ((crz * srx * sry - cry * srz) * pointOri.x +
                   (-cry * crz - srx * sry * srz) * pointOri.y) *
                      coeff.x +
                  (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y +
                  ((sry * srz + cry * crz * srx) * pointOri.x +
                   (crz * sry - cry * srx * srz) * pointOri.y) *
                      coeff.z;
        */
        float arx = ((crz*sry*crx + srz*srx)* pointOri.y +(srz*crx-crz*sry*srx)* pointOri.z)*coeff.x +
        ((srz*sry*crx-crz*srx)*pointOri.y -(srz*sry*srx+crz*crx)*pointOri.z)*coeff.y +
        (cry*crx*pointOri.y-cry*srx*pointOri.z)*coeff.z;

        float ary = (-crz*sry*pointOri.x+crz*cry*srx*pointOri.y+crz*cry*crx*pointOri.z)*coeff.x +
        (-srz*sry*pointOri.x+srz*cry*srx*pointOri.y +srz*cry*crx*pointOri.z)*coeff.y +
        (-cry*pointOri.x-sry*srx*pointOri.y-sry*crx*pointOri.z)*coeff.z;

        float arz = (-srz*cry*pointOri.x -(srz*sry*srx+crz*crx)*pointOri.y+(crz*srx-srz*sry*crx)*pointOri.z)*coeff.x+
        (crz*cry*pointOri.x+ (crz*sry*srx-srz*crx)*pointOri.y+crz*sry*crx+srz*srx*pointOri.z)*coeff.y+
        0*coeff.z;

      matA(i, 0) = arx;
      matA(i, 1) = ary;
      matA(i, 2) = arz;
      matA(i, 3) = coeff.x;
      matA(i, 4) = coeff.y;
      matA(i, 5) = coeff.z;
      matB(i, 0) = -coeff.intensity;
    }

    matAt = matA.transpose();
    matAtA = matAt * matA;
    matAtB = matAt * matB;
    matX = matAtA.colPivHouseholderQr().solve(matAtB);

    if (iterCount == 0) {
      Eigen::Matrix<float, 1, 6> matE;
      Eigen::Matrix<float, 6, 6> matV;
      Eigen::Matrix<float, 6, 6> matV2;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(matAtA);
      matE = esolver.eigenvalues().real();
      matV = esolver.eigenvectors().real();

      matV2 = matV;

      isDegenerate = false;
      float eignThre[6] = {100, 100, 100, 100, 100, 100};
      for (int i = 0; i < 6; i++) {
        if (matE(0, i) < eignThre[i]) {
          for (int j = 0; j < 6; j++) {
            matV2(i, j) = 0;
          }
          isDegenerate = true;
        } else {
          break;
        }
      }
      matP = matV.inverse() * matV2;
    }

    if (isDegenerate) {
      Eigen::Matrix<float, 6, 1> matX2(matX);
      matX = matP * matX2;
    }

    transform.rot_x += matX(0, 0);
    transform.rot_y += matX(1, 0);
    transform.rot_z += matX(2, 0);
    transform.pos.x() += matX(3, 0);
    transform.pos.y() += matX(4, 0);
    transform.pos.z() += matX(5, 0);

    float deltaR =
        sqrt(pow(rad2deg(matX(0, 0)), 2) + pow(rad2deg(matX(1, 0)), 2) +
             pow(rad2deg(matX(2, 0)), 2));
    float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + pow(matX(4, 0) * 100, 2) +
                        pow(matX(5, 0) * 100, 2));

    // std::cout << "iterator:" << iterCount << std::endl;
    // transform.print();
    if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
      converge = true;
      break;
    }
  }

  if (converge && _useScore) {

    double score = getScore(coeffSel);

    double match_count = line_match_count + plane_match_count;
    float percent = match_count / (CornerNum + SurfNum);
    std::cout << "scan match score:" << score << ",per:" << percent
              << std::endl;

    if (_fineScore) {
      laserCloudOri.clear();
      coeffSel.clear();
      line_match_count = 0;
      plane_match_count = 0;
      for (int i = 0; i < CornerNum; i++) {
        pointOri = CornerCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                    pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.02) {
          const Eigen::Vector3f &point = pointSel.getVector3fMap();
          Eigen::Vector3f lineA, lineB;
          if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
            PointI coefficients;
            if (getCornerFeatureCoefficients(lineA, lineB, point,
                                             coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            line_match_count++;
          }
        }
      }

      for (int i = 0; i < SurfNum; i++) {
        pointOri = SurfCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.05) {
          Eigen::Vector4f planeCoef;
          if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
            PointI coefficients;
            if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                              coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            plane_match_count++;
          }
        }
      }

      double score2 = getScore(coeffSel);
      match_count = line_match_count + plane_match_count;
      float percent2 = match_count / (CornerNum + SurfNum);
      std::cout << "scan match score2:" << score2 << " ,per2:" << percent2
                << std::endl;
    }

    if (score < _score_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low score!!" << score << ",per:" << percent << std::endl;
      return false;
    }

    if (percent < _match_percentage_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low percent!!" << score << ",per:" << percent << std::endl;
      return false;
    }
    _total_score += score;
    _match_count++;
    transformf = transform;

    return true;
  }
  //??? should we take the result when not converge?
  transformf = transform;
  _fail_match_count++;

  return false;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchScan(referenceCornerCloud, referenceSurfCloud,
                               CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchLocal(referenceCornerCloud, referenceSurfCloud,
                                CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Twist &transform) {
  _referenceCornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(referenceCornerCloud);
  _downSizeFilterCorner.filter(*_referenceCornerCloudDS);

  _referenceSurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(referenceSurfCloud);
  _downSizeFilterSurf.filter(*_referenceSurfCloudDS);

  _CornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(CornerCloud);
  _downSizeFilterCorner.filter(*_CornerCloudDS);

  _SurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(SurfCloud);
  _downSizeFilterSurf.filter(*_SurfCloudDS);

  return scanMatchScan(_referenceCornerCloudDS, _referenceSurfCloudDS,
                       _CornerCloudDS, _SurfCloudDS, transform);
}

} // end namespace lidar_slam

ext: cpp

max_stars_repo_name: autonomy-lab-cooper/CooperMapper

max_issues_repo_name: autonomy-lab-cooper/CooperMapper

max_forks_repo_name: autonomy-lab-cooper/CooperMapper


text:
#include <iostream>
#include <fstream>
#include <map>
#include "wcd.hxx"

#include <ff_to_aptk.hxx>
#include <strips_prob.hxx>
#include <fluent.hxx>
#include <action.hxx>
#include <cond_eff.hxx>
#include <strips_state.hxx>
#include <fwd_search_prob.hxx>
#include <mutex_set.hxx>

#include <h_1.hxx>
#include <rp_heuristic.hxx>
#include <h_2.hxx>

#include <aptk/open_list.hxx>
#include <aptk/string_conversions.hxx>
#include <aptk/at_bfs.hxx>

#include <fstream>

#include <boost/program_options.hpp>

namespace po = boost::program_options;

using	aptk::STRIPS_Problem;

using	aptk::agnostic::Fwd_Search_Problem;
using	aptk::Action;

using 	aptk::agnostic::H1_Heuristic;
using 	aptk::agnostic::H2_Heuristic;
using	aptk::agnostic::H_Add_Evaluation_Function;
using	aptk::agnostic::H_Max_Evaluation_Function;

using 	aptk::search::Open_List;
using	aptk::search::bfs::AT_BFS_SQ_SH;
using	aptk::search::Node_Comparer;

typedef	aptk::search::bfs::Node< aptk::State >	Search_Node;
typedef	Node_Comparer< Search_Node >	Tie_Breaking_Algorithm;
typedef	Open_List< Tie_Breaking_Algorithm, Search_Node >	BFS_Open_List;

typedef	H1_Heuristic<Fwd_Search_Problem, H_Max_Evaluation_Function>	H_Max_Fwd;
typedef	H2_Heuristic<Fwd_Search_Problem>	H2_FWD;
typedef	AT_BFS_SQ_SH< Fwd_Search_Problem, H_Max_Fwd, BFS_Open_List >		BFS_H_Max_Fwd;

using	aptk::agnostic::Relaxed_Plan_Heuristic;

typedef	H1_Heuristic<Fwd_Search_Problem, H_Add_Evaluation_Function>	H_Add_Fwd;
typedef	Relaxed_Plan_Heuristic< Fwd_Search_Problem, H_Add_Fwd >		H_Add_Rp_Fwd;

typedef AT_BFS_SQ_SH<Fwd_Search_Problem, H_Add_Rp_Fwd, BFS_Open_List> BFS_H_RP_Fwd;

void print_wcd_plan_to_cout(std::vector< aptk::Action_Idx >& plan, Fwd_Search_Problem& search_prob);

void do_wcd_search(  Fwd_Search_Problem& search_prob,  std::vector< aptk::Action_Idx >& plan, float& cost, std::string plan_filename,  bool anytime, bool delayed, bool greedy  ) {
    cost = infty;
    BFS_H_Max_Fwd engine( search_prob );
    engine.start();
    engine.find_solution( cost, plan );

    //print_wcd_plan_to_cout(plan, search_prob);

    //H_Add_Rp_Fwd h_ff(search_prob);
    //Search_Node* root = new Search_Node( search_prob.init(), 0.0f,-1, NULL );
    //std::vector<aptk::Action_Idx> pref_ops;
    //h_ff.eval(*(root->state()), cost, pref_ops, plan);

}

void translate_problem_latest_split(aptk::STRIPS_Problem& prob, aptk::STRIPS_Problem& trans_prob, std::map<std::string, unsigned int>& idx_to_trans_idx, float epsilon){

	aptk::Fluent_Vec init_trans;

	unsigned int index_0 = 0;
	unsigned int index_1 = 1;

	aptk::Fluent_Ptr_Vec f = prob.fluents();
	aptk::Fluent_Vec init = prob.init();
	aptk::Action_Ptr_Vec as = prob.actions();

	trans_prob.set_domain_name(prob.domain_name());
	trans_prob.set_problem_name(prob.problem_name());

	// Transformed fluent set vector contains two fluents for every one in the original. One for each goal.
	// Also contains the split and done_0 fluents.
 	for (unsigned int i = 0; i < prob.num_fluents(); i++){

		trans_prob.add_fluent(trans_prob, (*f[i]).signature() + "_0");
		idx_to_trans_idx.insert(std::pair<std::string, unsigned int>(std::to_string((*f[i]).index()) + "_0",index_0));

		trans_prob.add_fluent(trans_prob, (*f[i]).signature() + "_1");
		idx_to_trans_idx.insert(std::pair<std::string, unsigned int>(std::to_string((*f[i]).index()) + "_1",index_1));

		index_0 += 2;
		index_1 += 2;
	}

	trans_prob.add_fluent(trans_prob, "split");
	idx_to_trans_idx.insert(std::pair<std::string, unsigned int>("split", index_0++));
	trans_prob.add_fluent(trans_prob, "not_split");
	idx_to_trans_idx.insert(std::pair<std::string, unsigned int>("not_split", index_0++));
	trans_prob.add_fluent(trans_prob, "done_0");
	idx_to_trans_idx.insert(std::pair<std::string, unsigned int>("done_0", index_0++));
	trans_prob.add_fluent(trans_prob, "not_done_0");
	idx_to_trans_idx.insert(std::pair<std::string, unsigned int>("not_done_0", index_0++));

	for (unsigned int i = 0; i < init.size(); i++){
	// Transformed initial state vector contains two fluents for every one in the original. One for each goal.
		init_trans.push_back(idx_to_trans_idx[std::to_string(init[i]) + "_0"]);
		init_trans.push_back(idx_to_trans_idx[std::to_string(init[i]) + "_1"]);
	}
	init_trans.push_back(idx_to_trans_idx["not_split"]);
	init_trans.push_back(idx_to_trans_idx["not_done_0"]);

	trans_prob.set_init(trans_prob, init_trans);

	// Transformed action set vector contains three actions for every one in the original.
	// One for each goal and one for both.
	// Also contains the DoSplit and Done_0 actions.

	aptk::Fluent_Vec dosplit_pre;
	aptk::Fluent_Vec dosplit_add;
	aptk::Fluent_Vec dosplit_del;
	aptk::Fluent_Vec done_0_pre;
	aptk::Fluent_Vec done_0_add;
	aptk::Fluent_Vec done_0_del;

	// Conditional effects to be added later
	aptk::Conditional_Effect_Vec cond_eff;

	for (unsigned int i = 0; i < prob.num_actions(); i++){

		aptk::Action a = *as[i];
		aptk::Fluent_Vec pre_0;
		aptk::Fluent_Vec pre_1;
		aptk::Fluent_Vec pre_01;
		aptk::Fluent_Vec add_0;
		aptk::Fluent_Vec add_1;
		aptk::Fluent_Vec add_01;
		aptk::Fluent_Vec del_0;
		aptk::Fluent_Vec del_1;
		aptk::Fluent_Vec del_01;

	// Adds transformed fluents to the preconditions vector
		for (unsigned int i = 0; i < a.prec_vec().size(); i++){
			pre_0.push_back(idx_to_trans_idx[std::to_string(a.prec_vec()[i]) + "_0"]);
			pre_1.push_back(idx_to_trans_idx[std::to_string(a.prec_vec()[i]) + "_1"]);
			pre_01.push_back(idx_to_trans_idx[std::to_string(a.prec_vec()[i]) + "_0"]);
			pre_01.push_back(idx_to_trans_idx[std::to_string(a.prec_vec()[i]) + "_1"]);
		}

		pre_01.push_back(idx_to_trans_idx["not_split"]);
		pre_0.push_back(idx_to_trans_idx["split"]);
		pre_0.push_back(idx_to_trans_idx["not_done_0"]);
		pre_1.push_back(idx_to_trans_idx["split"]);
		pre_1.push_back(idx_to_trans_idx["done_0"]);

	// Adds transformed fluents to the add vector
		for (unsigned int i = 0; i < a.add_vec().size(); i++){
			add_0.push_back(idx_to_trans_idx[std::to_string(a.add_vec()[i]) + "_0"]);
			add_1.push_back(idx_to_trans_idx[std::to_string(a.add_vec()[i]) + "_1"]);
			add_01.push_back(idx_to_trans_idx[std::to_string(a.add_vec()[i]) + "_0"]);
			add_01.push_back(idx_to_trans_idx[std::to_string(a.add_vec()[i]) + "_1"]);
		}

	// Adds transformed fluents to the del vector
		for (unsigned int i = 0; i < a.del_vec().size(); i++){
			del_0.push_back(idx_to_trans_idx[std::to_string(a.del_vec()[i]) + "_0"]);
			del_1.push_back(idx_to_trans_idx[std::to_string(a.del_vec()[i]) + "_1"]);
			del_01.push_back(idx_to_trans_idx[std::to_string(a.del_vec()[i]) + "_0"]);
			del_01.push_back(idx_to_trans_idx[std::to_string(a.del_vec()[i]) + "_1"]);
		}

		trans_prob.add_action(trans_prob, a.signature() + "_0", pre_0, add_0, del_0, cond_eff, a.cost());
		trans_prob.add_action(trans_prob, a.signature() + "_1", pre_1, add_1, del_1, cond_eff, a.cost());
		trans_prob.add_action(trans_prob, a.signature() + "_01", pre_01, add_01, del_01, cond_eff, 2*a.cost() - epsilon);

	}

	done_0_pre.push_back(idx_to_trans_idx["split"]);
	dosplit_add.push_back(idx_to_trans_idx["split"]);
	done_0_add.push_back(idx_to_trans_idx["done_0"]);
	dosplit_del.push_back(idx_to_trans_idx["not_split"]);
	done_0_del.push_back(idx_to_trans_idx["not_done_0"]);

	trans_prob.add_action(trans_prob, "DoSplit", dosplit_pre, dosplit_add, dosplit_del, cond_eff, 0);
	trans_prob.add_action(trans_prob, "Done_0", done_0_pre, done_0_add, done_0_del, cond_eff, 0);

	trans_prob.make_action_tables();

}

void translate_goals(aptk::Fluent_Vec& g0, aptk::Fluent_Vec& g1, std::map<std::string, unsigned int>& idx_to_trans_idx, aptk::Fluent_Vec& trans_goal){

	// Adds transformed goals
	for (unsigned int i = 0; i < g0.size(); i++){
		trans_goal.push_back(idx_to_trans_idx[std::to_string(g0[i]) + "_0"]);
	}

	for (unsigned int i = 0; i < g1.size(); i++){
		trans_goal.push_back(idx_to_trans_idx[std::to_string(g1[i]) + "_1"]);
	}
}

void fluent_combinations(unsigned int offset, unsigned int arity, aptk::Fluent_Ptr_Vec& f, aptk::Fluent_Vec& curr_comb, std::vector<aptk::Fluent_Vec>& goal_set, H2_FWD& h2){
	if (arity == 0) {
		if (h2.eval(curr_comb) != infty)
			goal_set.push_back(curr_comb);
		return;
	}
	for (unsigned int i = offset; i <= f.size() - arity; ++i) {
		curr_comb.push_back((*f[i]).index());
		fluent_combinations(i + 1, arity - 1, f, curr_comb, goal_set, h2);
		curr_comb.pop_back();
	}
}

void print_goal_set(std::vector<aptk::Fluent_Vec>& goal_set, aptk::STRIPS_Problem& prob){
	for (unsigned int i = 0; i < goal_set.size(); i++){
		for (unsigned int j = 0; j < goal_set[i].size(); j++){
			std::cout << (*prob.fluents()[goal_set[i][j]]).signature() << " ";
		}
		std::cout << std::endl;
	}
}

void print_wcd_plan_to_cout(std::vector< aptk::Action_Idx >& plan, Fwd_Search_Problem& search_prob){
	for ( unsigned k = 0; k < plan.size(); k++ ) {
		const aptk::Action& a = *(search_prob.task().actions()[ plan[k] ]);
		std::cout  << a.signature() << ",";
	}
	std::cout  << std::endl;
}

void generate_goal_set(aptk::STRIPS_Problem& prob, int goal_arity, std::vector<aptk::Fluent_Vec>& goal_set){
	aptk::Fluent_Vec empty_temp;
	Fwd_Search_Problem fw(&prob);
	H2_FWD h2(fw);
	h2.compute_edeletes(prob);

	for (unsigned int i = goal_arity; i > 0; i--){
		fluent_combinations(0, i, prob.fluents(), empty_temp, goal_set, h2);
	}
}

int count_plan_until_split(std::vector< aptk::Action_Idx>& plan, Fwd_Search_Problem& search_prob){
	int count = 0;
	for ( unsigned int k = 0; k < plan.size(); k++ ) {
		const aptk::Action& a = *(search_prob.task().actions()[ plan[k] ]);
		if (a.signature() == "DoSplit")
			return count;
		else
			count++;
	}
	return count;
}

float wcd(aptk::STRIPS_Problem& prob, std::vector<aptk::Fluent_Vec>& goal_set, float epsilon){

	// This function implements the latest_split method for determing the WCD of a problem.
	// Keren, S., Gal, A., and Karpas, E.: ‘Goal recognition design’, International Conference on Automated Planning and Scheduling (ICAPS), 2014

	aptk::STRIPS_Problem trans_prob;
	std::map<std::string, unsigned int> idx_to_trans_idx;
	aptk::Fluent_Vec trans_goal;

  Fwd_Search_Problem	search_prob( &prob );
  float opt_cost = infty;
  std::vector< aptk::Action_Idx > plan;
  BFS_H_Max_Fwd engine( search_prob );
  engine.start();
  engine.find_solution( opt_cost, plan );

	translate_problem_latest_split(prob, trans_prob, idx_to_trans_idx, epsilon);

	unsigned int max = 0;

	for (unsigned int i = 0; i < goal_set.size(); i++){
		unsigned int wcd_g_val = wcd_g(trans_prob, idx_to_trans_idx, goal_set[i], goal_set, epsilon);
		max = (wcd_g_val > max) ? wcd_g_val : max;

		// So two goals are not compared twice
		goal_set.erase (goal_set.begin()+i);
	}

	return (float)( max + 1 ) / (float)opt_cost;
}

unsigned int wcd_g(aptk::STRIPS_Problem& trans_prob, std::map<std::string, unsigned int>& idx_to_trans_idx, aptk::Fluent_Vec& r_goal, std::vector<aptk::Fluent_Vec>& goal_set, float epsilon){

	// This function implements the latest_split method for determing the WCD of a problem with reguards to a single goal.
	// Keren, S., Gal, A., and Karpas, E.: ‘Goal recognition design’, International Conference on Automated Planning and Scheduling (ICAPS), 2014

	aptk::Fluent_Vec trans_goal;
	unsigned int max = 0;

	for (unsigned int i = 0; i < goal_set.size(); i++){
			if (goal_set[i] != r_goal) {

  				aptk::Fluent_Vec trans_goal;
				std::vector< aptk::Action_Idx > plan;
				float cost = 0;

				translate_goals(goal_set[i], r_goal, idx_to_trans_idx, trans_goal);
				trans_prob.set_goal(trans_prob, trans_goal);

				Fwd_Search_Problem	search_prob( &trans_prob );
				do_wcd_search(search_prob, plan, cost, "plan.ipc" ,  0, 0, 0 );

        //for(unsigned int i = 0; i < search_prob.task().fluents().size(); i++){
          //std::cout << search_prob.task().fluents()[i]->signature() << std::endl;
        //}
        //for(unsigned int i = 0; i < search_prob.task().actions().size(); i++){
          //std::cout << search_prob.task().actions()[i]->signature() << std::endl;
        //}

				unsigned int until_split = count_plan_until_split(plan, search_prob);
				max = (until_split > max) ? until_split : max;
			}
	}

	return max;
}

ext: cxx


text:
#include <iostream>
#include <map>
#include <string>
#include <vector>
#include <filesystem>

#include <wx/msgdlg.h>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/tokenizer.hpp>

struct ScriptInfo
{
	std::string sName;
	std::string sDescription;
	std::vector<std::string> aTags;
};

std::map<std::string, std::function<std::string(const std::string&)>> dialogGenMap =
{
	{
		"OK",
		[](const std::string& sText)
		{
			wxMessageDialog(nullptr, sText, "").ShowModal();
			return "";
		}
	},
	{
		"YesNo",
		[](const std::string& sQuestion)
		{
			int nRes = wxMessageDialog(nullptr, sQuestion, "", wxYES_NO).ShowModal();

			if (nRes == wxID_YES)
				return "Yes";
			else if (nRes == wxID_NO)
				return "No";
			else
				return "";
		}
	}
};

int main(int argc, char** argv)
{
	boost::property_tree::ptree pt;
	boost::property_tree::ptree flowRoot;

	std::vector<ScriptInfo> scriptList;
	std::map<std::string, std::vector<ScriptInfo*>> tagMap;

	//List out the product options
	if (argc > 1)
	{
		std::cout << "Opening directory " << argv[1] << std::endl;
		std::cout << "Products found:" << std::endl;
		for (auto it = std::filesystem::directory_iterator(argv[1]); it != std::filesystem::directory_iterator(); it++)
		{
			std::filesystem::path p = *it;
			if (it->is_directory() && p.extension().string() == ".product")
				std::cout << p.filename().string() << std::endl;
		}

		std::cout << std::endl;
	}

	//List out the scripts for a specific product
	if (argc > 2)
	{
		//Open the product specific directory
		std::filesystem::path productsPath(argv[1]);
		productsPath /= argv[2];

		std::cout << "Opening product " << argv[2] << std::endl;
		std::cout << "Scripts found:" << std::endl;
		for (auto it = std::filesystem::directory_iterator(productsPath); it != std::filesystem::directory_iterator(); it++)
		{
			std::filesystem::path p = *it;
			//It needs to be a .script folder
			if (it->is_directory() && p.extension().string() == ".script")
			{
				boost::property_tree::ptree pt;

				//Build up the script info list
				try
				{
					ScriptInfo scriptInfo;
					typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
					boost::char_separator<char> token(",");

					//Grab the actual script info
					boost::property_tree::read_xml(p.string() + "\\script.xml", pt);
					std::cout << p.filename().string() << std::endl;

					scriptInfo.sName = p.filename().string();
					scriptInfo.sDescription = pt.get<std::string>("Description", "");
					//Tags are comma separated
					std::string sTags = pt.get<std::string>("Tags", "");

					//Tokenize tag list
					tokenizer tokens(sTags, token);
					for (tokenizer::iterator it = tokens.begin(); it != tokens.end(); it++)
						scriptInfo.aTags.push_back(*it);

					//Add script info to list
					scriptList.push_back(scriptInfo);
				}
				catch (boost::property_tree::ptree_error& e)
				{
					std::cout << "Could not parse " << p.filename().string() << std::endl;
				}
			}
		}

		std::cout << std::endl;
	}

	if (pt.get_child_optional("flow"))
	{
		bool bPathDefined = true;
		flowRoot = pt.get_child("flow");

		while (bPathDefined)
		{
			std::string sType = flowRoot.get<std::string>("Type", "");
			std::string sRes = "";

			bPathDefined = false;
			if (dialogGenMap.find(sType) != dialogGenMap.end())
				sRes = dialogGenMap[sType](flowRoot.get<std::string>("Text", ""));
			else
				dialogGenMap["OK"]("No dialog defined");

			if (!sRes.empty() && flowRoot.get_child_optional(sRes))
			{
				bPathDefined = true;
				flowRoot = flowRoot.get_child(sRes);
			}

			if (!bPathDefined)
				dialogGenMap["OK"]("No path defined");
		}

	}
	else
	{
		dialogGenMap["OK"]("No flow defined");
	}

	return 0;
}

max_stars_repo_name: humanHardDrive/HelpMe

max_issues_repo_name: humanHardDrive/HelpMe

max_forks_repo_name: humanHardDrive/HelpMe


text:
// FILE NAME:       $HeadURL: svn+ssh://svn.cm.aol.com/advertising/adlearn/gen1/trunk/lib/cpp/DataProxy/test/RestDataProxyTest.hpp $
//
// REVISION:        $Revision: 280002 $
//
// COPYRIGHT:       (c) 2006 Advertising.com All Rights Reserved.
//
// LAST UPDATED:    $Date: 2013-06-03 15:11:03 -0400 (Mon, 03 Jun 2013) $
// UPDATED BY:      $Author: sstrick $

#ifndef _REST_DATA_PROXY_TEST_HPP_
#define _REST_DATA_PROXY_TEST_HPP_

#include <cppunit/TestFixture.h>
#include <cppunit/extensions/HelperMacros.h>
#include <boost/scoped_ptr.hpp>
#include "RestDataProxy.hpp"

class TempDirectory;
class SimpleRestMockService;

class RestDataProxyTest : public CppUnit::TestFixture
{
private:
	CPPUNIT_TEST_SUITE( RestDataProxyTest );
	CPPUNIT_TEST( testOperationAttributeParsing );
	CPPUNIT_TEST( testMissingLocation );
	CPPUNIT_TEST( testMoreThanOneUriQueryParametersNode );
	CPPUNIT_TEST( testMoreThanOneHttpHeaderParametersNode );
	CPPUNIT_TEST( testMoreThanOneUriPathSegmentParametersNode );
	CPPUNIT_TEST( testMalformedReadNode );
	CPPUNIT_TEST( testMalformedWriteNode );
	CPPUNIT_TEST( testMalformedDeleteNode );
	CPPUNIT_TEST( testMalformedUriQueryParametersNode );
	CPPUNIT_TEST( testMalformedUriQueryGroupParametersNode );
	CPPUNIT_TEST( testMalformedHttpHeaderParametersNode );
	CPPUNIT_TEST( testMalformedUriPathSegmentParametersNode );
	CPPUNIT_TEST( testMalformedParameterNode );
	CPPUNIT_TEST( testDuplicateParameters );
	CPPUNIT_TEST( testPing );
	CPPUNIT_TEST( testLoadTimeout );
	CPPUNIT_TEST( testLoadBasic );
	CPPUNIT_TEST( testLoadMethodOverride );
	CPPUNIT_TEST( testLoadComplex );
	CPPUNIT_TEST( testLoadWithBodyParameter );
	CPPUNIT_TEST( testStoreTimeout );
	CPPUNIT_TEST( testStoreBasic );
	CPPUNIT_TEST( testStoreMethodOverride );
	CPPUNIT_TEST( testStoreWithBodyParameter );
	CPPUNIT_TEST( testStoreComplex );
	CPPUNIT_TEST( testDeleteTimeout );
	CPPUNIT_TEST( testDeleteBasic );
	CPPUNIT_TEST( testDeleteMethodOverride );
	CPPUNIT_TEST( testDeleteComplex );
	CPPUNIT_TEST( testDeleteWithBodyParameter );
	CPPUNIT_TEST_SUITE_END();

public:
	RestDataProxyTest();
	virtual ~RestDataProxyTest();

	void setUp();
	void tearDown();

	void testOperationAttributeParsing();
	void testMissingLocation();
	void testMoreThanOneUriQueryParametersNode();
	void testMoreThanOneHttpHeaderParametersNode();
	void testMoreThanOneUriPathSegmentParametersNode();
	void testMalformedReadNode();
	void testMalformedWriteNode();
	void testMalformedDeleteNode();
	void testMalformedUriQueryParametersNode();
	void testMalformedUriQueryGroupParametersNode();
	void testMalformedHttpHeaderParametersNode();
	void testMalformedUriPathSegmentParametersNode();
	void testMalformedParameterNode();
	void testDuplicateParameters();
	void testPing();
	void testLoadTimeout();
	void testLoadBasic();
	void testLoadMethodOverride();
	void testLoadComplex();
	void testLoadWithBodyParameter();
	void testStoreTimeout();
	void testStoreBasic();
	void testStoreMethodOverride();
	void testStoreComplex();
	void testStoreWithBodyParameter();
	void testDeleteTimeout();
	void testDeleteBasic();
	void testDeleteMethodOverride();
	void testDeleteComplex();
	void testDeleteWithBodyParameter();

private:
	boost::scoped_ptr<TempDirectory> m_pTempDir;
	boost::scoped_ptr<SimpleRestMockService> m_pService;
};

#endif //_REST_DATA_PROXY_TEST_HPP_

ext: hpp

max_stars_repo_name: aol/DataProxyLibrary

max_issues_repo_name: aol/DataProxyLibrary

max_forks_repo_name: aol/DataProxyLibrary


text:
#include <boost/array.hpp>

int main()
{
  boost::array<int, 3> a{};
  return a[0];
}

ext: cc


text:
// Copyright 2004-present Facebook. All Rights Reserved.

#include "fboss/agent/hw/bcm/BcmMirror.h"
#include "fboss/agent/hw/bcm/BcmAclTable.h"
#include "fboss/agent/hw/bcm/BcmPlatform.h"
#include "fboss/agent/hw/bcm/BcmPortTable.h"
#include "fboss/agent/hw/bcm/BcmSwitch.h"

#include "fboss/agent/hw/bcm/BcmAddressFBConvertors.h"
#include "fboss/agent/hw/bcm/BcmError.h"
#include "fboss/agent/hw/bcm/BcmMirrorUtils.h"
#include "fboss/agent/hw/bcm/BcmWarmBootCache.h"
#include "fboss/agent/hw/switch_asics/HwAsic.h"
#include "fboss/agent/state/Mirror.h"

#include <boost/container/flat_map.hpp>

#include <folly/MacAddress.h>
#include <folly/logging/xlog.h>
#include <optional>

namespace {
int getMirrorFlags(const std::optional<facebook::fboss::MirrorTunnel>& tunnel) {
  if (!tunnel) {
    return 0;
  }
  if (tunnel->udpPorts) {
    return BCM_MIRROR_DEST_TUNNEL_SFLOW;
  }
  return BCM_MIRROR_DEST_TUNNEL_IP_GRE;
}
} // namespace
namespace facebook::fboss {

BcmMirrorDestination::BcmMirrorDestination(
    int unit,
    BcmPort* egressPort,
    uint8_t dscp,
    bool truncate)
    : unit_(unit) {
  bcm_mirror_destination_t mirror_destination;
  bcm_mirror_destination_t_init(&mirror_destination);
  mirror_destination.gport = egressPort->getBcmGport();
  mirror_destination.tos = dscp;
  if (truncate) {
    mirror_destination.truncate = BCM_MIRROR_PAYLOAD_TRUNCATE;
  }
  auto rv = bcm_mirror_destination_create(unit_, &mirror_destination);
  bcmCheckError(rv, "failed to create mirror on port");
  handle_ = mirror_destination.mirror_dest_id;
  flags_ = mirror_destination.flags;
}

BcmMirrorDestination::BcmMirrorDestination(
    int unit,
    BcmPort* egressPort,
    const MirrorTunnel& mirrorTunnel,
    uint8_t dscp,
    bool truncate)
    : unit_(unit) {
  bcm_mirror_destination_t mirror_destination;
  bcm_mirror_destination_t_init(&mirror_destination);
  mirror_destination.gport = egressPort->getBcmGport();
  if (mirrorTunnel.udpPorts.has_value()) {
    mirror_destination.flags = BCM_MIRROR_DEST_TUNNEL_SFLOW;
    mirror_destination.udp_src_port = mirrorTunnel.udpPorts.value().udpSrcPort;
    mirror_destination.udp_dst_port = mirrorTunnel.udpPorts.value().udpDstPort;
    XLOG(INFO) << "SFLOW tunnel: source([" << mirrorTunnel.srcIp
               << "]:" << mirror_destination.udp_src_port << "/"
               << mirrorTunnel.srcMac << "), destination(["
               << mirrorTunnel.dstIp << "]:" << mirror_destination.udp_dst_port
               << "/" << mirrorTunnel.dstMac << ")";

  } else {
    mirror_destination.gre_protocol = mirrorTunnel.greProtocol;
    mirror_destination.flags = BCM_MIRROR_DEST_TUNNEL_IP_GRE;
    XLOG(INFO) << "ERSPAN(I) tunnel: source(" << mirrorTunnel.srcIp << "/"
               << mirrorTunnel.srcMac << "), destination(" << mirrorTunnel.dstIp
               << "/" << mirrorTunnel.dstMac << ")";
  }
  mirror_destination.ttl = mirrorTunnel.ttl;
  mirror_destination.tos = dscp;
  if (truncate) {
    mirror_destination.truncate = BCM_MIRROR_PAYLOAD_TRUNCATE;
  }
  macToBcm(mirrorTunnel.srcMac, &mirror_destination.src_mac);
  macToBcm(mirrorTunnel.dstMac, &mirror_destination.dst_mac);

  if (mirrorTunnel.dstIp.isV4()) {
    mirror_destination.version = 4;

    mirror_destination.src_addr = mirrorTunnel.srcIp.asV4().toLongHBO();
    mirror_destination.dst_addr = mirrorTunnel.dstIp.asV4().toLongHBO();
  } else {
    mirror_destination.version = 6;
    ipToBcmIp6(mirrorTunnel.srcIp, &mirror_destination.src6_addr);
    ipToBcmIp6(mirrorTunnel.dstIp, &mirror_destination.dst6_addr);
  }
  auto rv = bcm_mirror_destination_create(unit_, &mirror_destination);
  bcmCheckError(rv, "failed to create mirror to ", mirrorTunnel.dstIp.str());
  handle_ = mirror_destination.mirror_dest_id;
  flags_ = mirror_destination.flags;
}

BcmMirrorHandle BcmMirrorDestination::getHandle() {
  return handle_;
}

BcmMirrorDestination::~BcmMirrorDestination() {
  auto rv = bcm_mirror_destination_destroy(unit_, handle_);
  bcmCheckError(rv, "failed to remove mirror ");
}

BcmMirror::BcmMirror(BcmSwitch* hw, const std::shared_ptr<Mirror>& mirror)
    : hw_(hw), mirrorName_(mirror->getID()) {
  program(mirror);
  // TODO(pshaikh) : if the port action apply fails for any of the port and
  // exception is thrown, mirror action stop should be applied.
  // To achieve this, maintain list of ports/acls mirrored and unmirrored and in
  // destructor use it. in MirrorAction::START, fill up that list in
  // MirrorAction::STOP, clear that list
  applyPortMirrorActions(MirrorAction::START);
  applyAclMirrorActions(MirrorAction::START);
}

BcmMirror::~BcmMirror() {
  // before unmirroring acls or ports, check if acl/port table are not deleted
  // this is required because of the order of table deletions, mirror table
  // is deleted after acl and port tables, but this is still needed to consider
  // the case of mirror update either due to resolution update or config update.
  if (hw_->getPortTable()) {
    applyPortMirrorActions(MirrorAction::STOP);
  }
  if (hw_->getAclTable()) {
    applyAclMirrorActions(MirrorAction::STOP);
  }
  XLOG(DBG3) << "destroyed mirror " << mirrorName_;
}

void BcmMirror::program(const std::shared_ptr<Mirror>& mirror) {
  if (!mirror->isResolved()) {
    return;
  }

  CHECK(!destination_);
  auto* bcmPort =
      hw_->getPortTable()->getBcmPortIf(mirror->getEgressPort().value());
  auto* warmBootCache = hw_->getWarmBootCache();
  auto iter = warmBootCache->findMirror(
      bcmPort->getBcmGport(), mirror->getMirrorTunnel());
  if (iter != warmBootCache->mirrorsEnd()) {
    destination_ = std::make_unique<BcmMirrorDestination>(
        hw_->getUnit(),
        iter->second,
        getMirrorFlags(mirror->getMirrorTunnel()));
    warmBootCache->programmedMirror(iter);
    XLOG(DBG3) << "found mirror " << mirrorName_
               << " with handle: " << destination_->getHandle()
               << " in warmboot cache";
    return;
  }
  bool truncate = mirror->getTruncate();
  if (truncate) {
    CHECK(hw_->getPlatform()->getAsic()->isSupported(
        HwAsic::Feature::MIRROR_PACKET_TRUNCATION))
        << "Mirrored packet truncation is not supported on this platform";
  }

  if (mirror->getMirrorTunnel().has_value()) {
    destination_ = std::make_unique<BcmMirrorDestination>(
        hw_->getUnit(),
        bcmPort,
        mirror->getMirrorTunnel().value(),
        mirror->getDscp(),
        truncate);
  } else {
    destination_ = std::make_unique<BcmMirrorDestination>(
        hw_->getUnit(), bcmPort, mirror->getDscp(), truncate);
  }
  XLOG(DBG3) << "created mirror " << mirrorName_
             << " with handle: " << destination_->getHandle();
}

void BcmMirror::applyPortMirrorAction(
    PortID port,
    MirrorAction action,
    MirrorDirection direction,
    cfg::SampleDestination sampleDest) {
  if (!isProgrammed()) {
    return;
  }
  uint32_t flags = directionToBcmPortMirrorFlag(direction) |
      sampleDestinationToBcmPortMirrorSflowFlag(sampleDest);
  auto handle = destination_->getHandle();
  switch (action) {
    case MirrorAction::START: {
      auto* warmBootCache = hw_->getWarmBootCache();
      auto* bcmPort = hw_->getPortTable()->getBcmPort(port);
      auto iter =
          warmBootCache->findMirroredPort(bcmPort->getBcmGport(), flags);
      if (iter != warmBootCache->mirroredPortsEnd()) {
        CHECK_EQ(handle, iter->second);
        warmBootCache->programmedMirroredPort(iter);
      } else {
        /* ports are not associated individually with sflow mirror for sampling.
         * doing so would cause _E_EXISTS error. ASIC behaves differently for
         * sflow sampled ports. Once first port-mirror mapping for one port is
         * established, it is also established for all other ports. to avoid
         * this unusual behavior, mirror-port association is done only for very
         * first port. */
        if (destination_->getFlags() & BCM_MIRROR_DEST_TUNNEL_SFLOW) {
          if (mirroredPorts_ > 0) {
            XLOG(DBG3) << "skipped " << mirrorDirectionName(direction)
                       << " sflow mirror: " << mirrorName_
                       << " on port: " << port;
          } else {
            /* add sflow mirror for obly the first port. this will activate
             * sflow mirror for all ports */
            bcmCheckError(
                bcm_mirror_port_dest_add(
                    hw_->getUnit(), BCM_PORT_INVALID, flags, handle),
                "failed to add port mapping for sflow mirror");
          }
        } else {
          bcmCheckError(
              bcm_mirror_port_dest_add(
                  hw_->getUnit(), static_cast<int>(port), flags, handle),
              "failed to add port ",
              port,
              " to ",
              mirrorDirectionName(direction),
              " mirror ",
              mirrorName_);
          XLOG(DBG3) << "added " << mirrorDirectionName(direction)
                     << " mirror: " << mirrorName_ << " on port: " << port;
        }
      }
      mirroredPorts_++;
    } break;
    case MirrorAction::STOP: {
      if (destination_->getFlags() & BCM_MIRROR_DEST_TUNNEL_SFLOW) {
        /*  since ports are not associated individually with sflow mirror for
         * sampling as doing so cause _E_EXISTS error(described above).
         * accordingly, mirror-port association is not removed individually.
         * instead a mirror-port mapping is removed only when no port refers
         * this mirror. this also deletes mapping for all other ports
         * referencing the mirror */
        if (mirroredPorts_ > 1) {
          XLOG(DBG3) << "skipped " << mirrorDirectionName(direction)
                     << " sflow mirror: " << mirrorName_
                     << " on port: " << port;
        } else if (mirroredPorts_ == 1) {
          /* remove sflow mirror for the last port. this will will deactivate
           * for sflow mirror for all port */
          bcmCheckError(
              bcm_mirror_port_dest_delete(
                  hw_->getUnit(), BCM_PORT_INVALID, flags, handle),
              "failed to delete port mapping for sflow mirror");
        }
      } else {
        bcmCheckError(
            bcm_mirror_port_dest_delete(
                hw_->getUnit(), static_cast<int>(port), flags, handle),
            "failed to delete port ",
            port,
            " from ",
            mirrorDirectionName(direction),
            " mirror ",
            mirrorName_);
        XLOG(DBG3) << "removed " << mirrorDirectionName(direction)
                   << " mirror: " << mirrorName_ << " on port: " << port;
      }
      mirroredPorts_--;
    } break;
  }
}

void BcmMirror::applyAclMirrorAction(
    BcmAclEntryHandle aclEntryHandle,
    MirrorAction action,
    MirrorDirection direction) {
  if (!isProgrammed()) {
    return;
  }

  auto handle = destination_->getHandle();
  switch (action) {
    case MirrorAction::START: {
      auto* warmBootCache = hw_->getWarmBootCache();
      auto iter = warmBootCache->findMirroredAcl(aclEntryHandle, direction);
      if (iter != warmBootCache->mirroredAclsEnd()) {
        CHECK_EQ(handle, iter->second);
        warmBootCache->programmedMirroredAcl(iter);
        return;
      } else {
        bcmCheckError(
            bcm_field_action_add(
                hw_->getUnit(),
                aclEntryHandle,
                directionToBcmAclMirrorAction(direction),
                0,
                handle),
            "failed to add acl ",
            aclEntryHandle,
            " to ",
            mirrorDirectionName(direction),
            " mirror ",
            mirrorName_);
      }
      XLOG(DBG3) << "added " << mirrorDirectionName(direction)
                 << " mirror: " << mirrorName_ << " on acl: " << aclEntryHandle;

    } break;
    case MirrorAction::STOP:
      bcmCheckError(
          bcm_field_action_delete(
              hw_->getUnit(),
              aclEntryHandle,
              directionToBcmAclMirrorAction(direction),
              0,
              handle),
          "failed to delete acl ",
          aclEntryHandle,
          " from ",
          mirrorDirectionName(direction),
          " mirror ",
          mirrorName_);
      XLOG(DBG3) << "removed " << mirrorDirectionName(direction)
                 << " mirror: " << mirrorName_ << " on acl: " << aclEntryHandle;

      break;
  }
  auto rv = bcm_field_entry_install(hw_->getUnit(), aclEntryHandle);
  bcmCheckError(rv, "failed to reload acl ", aclEntryHandle);
}

void BcmMirror::applyPortMirrorActions(MirrorAction action) {
  for (const auto& [portID, bcmPort] : *hw_->getPortTable()) {
    if (bcmPort->getIngressPortMirror() == mirrorName_) {
      applyPortMirrorAction(
          portID,
          action,
          MirrorDirection::INGRESS,
          bcmPort->getSampleDestination());
    }
    if (bcmPort->getEgressPortMirror() == mirrorName_) {
      applyPortMirrorAction(
          portID,
          action,
          MirrorDirection::EGRESS,
          bcmPort->getSampleDestination());
    }
  }
}

void BcmMirror::applyAclMirrorActions(MirrorAction action) {
  hw_->getAclTable()->forFilteredEach(
      [=](const auto& aclTableEntry) {
        const auto& bcmAclEntry = aclTableEntry.second;
        return bcmAclEntry->getIngressAclMirror() == mirrorName_ ||
            bcmAclEntry->getEgressAclMirror() == mirrorName_;
      },
      [=](const auto& aclTableEntry) {
        const auto& bcmAclEntry = aclTableEntry.second;
        const auto bcmAclEntryHandle = bcmAclEntry->getHandle();

        if (bcmAclEntry->getIngressAclMirror() == mirrorName_) {
          applyAclMirrorAction(
              bcmAclEntryHandle, action, MirrorDirection::INGRESS);
        }
        if (bcmAclEntry->getEgressAclMirror() == mirrorName_) {
          applyAclMirrorAction(
              bcmAclEntryHandle, action, MirrorDirection::EGRESS);
        }
      });
}

} // namespace facebook::fboss

max_stars_repo_name: dgrnbrg-meta/fboss

max_issues_repo_name: dgrnbrg-meta/fboss

max_forks_repo_name: dgrnbrg-meta/fboss


text:
//
// Created by think on 2020/12/17.
//

#include <yaml-cpp/yaml.h>
#include <iostream>
#include "Model.h"
#include "Link.h"
#include <Eigen/Dense>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv)
{
    YAML::Node config = YAML::LoadFile("../../res/iiwa/config.yaml");

    if(config["robot"].IsDefined()) {
        std::cout << "robot info: " << std::endl;
    }

    YAML::Node info = config["robot"];
    assert(info.Type() == YAML::NodeType::Sequence);

    std::cout << "info size is: " << info.size() << std::endl;
    std::cout << "robot freedom is " << info.size() - 1 << std::endl;

    RHCL::Model model("../../res/staubli_tx60l/config.yaml");

    std::vector<double> jntRads(model.getFreedom(), 0);

    RHCL::PointCloudPtr pc = model.getPointCloud(jntRads);

    std::cout << "Point cloud size is: " << pc->size() << std::endl;

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("viewer1"));

    //source点云窗口
    viewer1->setBackgroundColor(255, 255, 255);//设置背景色为白色
    viewer1->addText("source_point_cloud_image", 10, 10,1.0,0.0,0.0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBA> target_color(pc, 255, 0, 0);
    viewer1->addPointCloud(pc, target_color, "source_point_cloud");

    while (!viewer1->wasStopped())
    {
        viewer1->spinOnce(100);   //100??
        std::this_thread::sleep_for(std::chrono::microseconds(100000));
//        std::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

//    for (int j = 0; j < info.size(); ++j) {
//        std::cout << " - ";
//        std::cout << "\t name: " << info[j]["name"] << std::endl;
//        std::cout << "\t order: " << info[j]["order"].as<int>() << std::endl;
//        if(info[j]["translate"].IsDefined())
//            std::cout << "\t translate: " << info[j]["translate"][0].as<double>()
//                    << ", " << info[j]["translate"][1].as<double>()
//                    << ", " << info[j]["translate"][2].as<double>() << std::endl;
//        if(info[j]["angleAxis"].IsDefined())
//            std::cout << "\t angleAxis: " << info[j]["angleAxis"][0].as<int>()
//                      << ", " << info[j]["angleAxis"][1].as<int>()
//                      << ", " << info[j]["angleAxis"][2].as<int>() << std::endl;
//        std::cout << "\t mesh: " << info[j]["mesh"] << std::endl;
//    }

    return 0;
}


text:
/**
 * @file drusilla_select_test.cpp
 * @author Ryan Curtin
 *
 * Test for DrusillaSelect.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/methods/approx_kfn/drusilla_select.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::neighbor;

BOOST_AUTO_TEST_SUITE(DrusillaSelectTest);

// If we have a dataset with an extreme outlier, then every point (except that
// one) should end up with that point as the furthest neighbor candidate.
BOOST_AUTO_TEST_CASE(DrusillaSelectExtremeOutlierTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  dataset.col(99) += 100; // Make last column very large.

  // Construct with some reasonable parameters.
  DrusillaSelect<> ds(dataset, 5, 5);

  // Query with every point except the extreme point.
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset.cols(0, 98), 1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 99);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 99);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);

  for (size_t i = 0; i < 99; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], 99);
  }
}

// If we use only one projection with the number of points equal to what is in
// the dataset, we should end up with the exact result.
BOOST_AUTO_TEST_CASE(DrusillaSelectExhaustiveExactTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);

  // Construct with one projection and 100 points in that projection.
  DrusillaSelect<> ds(dataset, 100, 1);

  arma::mat distances, distancesTrue;
  arma::Mat<size_t> neighbors, neighborsTrue;

  ds.Search(dataset, 5, neighbors, distances);

  KFN kfn(dataset);
  kfn.Search(dataset, 5, neighborsTrue, distancesTrue);

  BOOST_REQUIRE_EQUAL(neighborsTrue.n_cols, neighbors.n_cols);
  BOOST_REQUIRE_EQUAL(neighborsTrue.n_rows, neighbors.n_rows);
  BOOST_REQUIRE_EQUAL(distancesTrue.n_cols, distances.n_cols);
  BOOST_REQUIRE_EQUAL(distancesTrue.n_rows, distances.n_rows);

  for (size_t i = 0; i < distances.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsTrue[i]);
    BOOST_REQUIRE_CLOSE(distances[i], distancesTrue[i], 1e-5);
  }
}

// Test that we can call Train() after calling the constructor.
BOOST_AUTO_TEST_CASE(RetrainTest)
{
  arma::mat firstDataset = arma::randu<arma::mat>(3, 10);
  arma::mat dataset = arma::randu<arma::mat>(3, 200);

  DrusillaSelect<> ds(firstDataset, 3, 3);
  ds.Train(std::move(dataset), 2, 2);

  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset, 1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 200);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 200);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);
}

// Test serialization.
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  // Create a random dataset.
  arma::mat dataset = arma::randu<arma::mat>(3, 100);

  DrusillaSelect<> ds(dataset, 3, 3);

  arma::mat fakeDataset1 = arma::randu<arma::mat>(2, 15);
  arma::mat fakeDataset2 = arma::randu<arma::mat>(10, 18);
  DrusillaSelect<> dsXml(fakeDataset1, 5, 3);
  DrusillaSelect<> dsText(2, 2);
  DrusillaSelect<> dsBinary(5, 2);
  dsBinary.Train(fakeDataset2);

  // Now do the serialization.
  SerializeObjectAll(ds, dsXml, dsText, dsBinary);

  // Now do a search and make sure all the results are the same.
  arma::Mat<size_t> neighbors, neighborsXml, neighborsText, neighborsBinary;
  arma::mat distances, distancesXml, distancesText, distancesBinary;

  ds.Search(dataset, 3, neighbors, distances);
  dsXml.Search(dataset, 3, neighborsXml, distancesXml);
  dsText.Search(dataset, 3, neighborsText, distancesText);
  dsBinary.Search(dataset, 3, neighborsBinary, distancesBinary);

  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsXml.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsXml.n_cols);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsText.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsText.n_cols);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsBinary.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsBinary.n_cols);

  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesXml.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesXml.n_cols);
  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesText.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesText.n_cols);
  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesBinary.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesBinary.n_cols);

  for (size_t i = 0; i < neighbors.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsXml[i]);
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsText[i]);
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsBinary[i]);

    BOOST_REQUIRE_CLOSE(distances[i], distancesXml[i], 1e-5);
    BOOST_REQUIRE_CLOSE(distances[i], distancesText[i], 1e-5);
    BOOST_REQUIRE_CLOSE(distances[i], distancesBinary[i], 1e-5);
  }
}

// Make sure we can create the object with a sparse matrix.
BOOST_AUTO_TEST_CASE(SparseTest)
{
  arma::sp_mat dataset;
  dataset.sprandu(50, 1000, 0.3);

  DrusillaSelect<arma::sp_mat> ds(dataset, 5, 10);

  // Run a search.
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 1000);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 1000);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

BOOST_AUTO_TEST_SUITE_END();

max_stars_repo_name: RMaron/mlpack

max_issues_repo_name: RMaron/mlpack

max_forks_repo_name: RMaron/mlpack


text:
/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_META_ALL_HPP_INCLUDED
#define NT2_SDK_META_ALL_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Check if all elements of a sequence matches some lambda function
// Documentation: http://nt2.lri.fr/sdk/meta/algo/all.html
////////////////////////////////////////////////////////////////////////////////
#include <boost/config.hpp>
#include <nt2/sdk/meta/details/all.hpp>

#if !defined(BOOST_HAS_VARIADIC_TMPL)
#include <boost/preprocessor/facilities/intercept.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/enum_binary_params.hpp>
#endif

namespace nt2 { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Recursive variadics
  //////////////////////////////////////////////////////////////////////////////
  #if defined(BOOST_HAS_VARIADIC_TMPL)
  template< class Pred, class... Args>
  struct  all : details::all_impl<Pred, Args...> {};
  #else
  //////////////////////////////////////////////////////////////////////////////
  // Recursive macro
  //////////////////////////////////////////////////////////////////////////////
  template< class Pred
          , BOOST_PP_ENUM_BINARY_PARAMS ( NT2_MAX_META_ARITY
                                        , class A
                                        , = na_ BOOST_PP_INTERCEPT
                                        )
          >
  struct  all
        : details::all_impl<Pred,BOOST_PP_ENUM_PARAMS(NT2_MAX_META_ARITY, A)>
  {};
  #endif
} }

#endif


text:
/*
Copyright Barrett Adair 2016-2017
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http ://boost.org/LICENSE_1_0.txt)
*/

#include <type_traits>
#include <functional>
#include <tuple>
#include <utility>
#include <boost/callable_traits.hpp>

using namespace boost::callable_traits;

#ifndef CT_ASSERT
#define CT_ASSERT(...) static_assert(__VA_ARGS__, #__VA_ARGS__)
#endif //CT_ASSERT

#ifdef BOOST_CLBL_TRTS_DISABLE_REFERENCE_QUALIFIERS
#define LREF
#define RREF
#else
#define LREF &
#define RREF &&
#endif

#define TX_SAFE BOOST_CLBL_TRTS_TRANSACTION_SAFE_SPECIFIER
#define VA_CC BOOST_CLBL_TRTS_DEFAULT_VARARGS_CC

#ifndef PP_CAT
#define PP_CAT_(x, y) x ## y
#define PP_CAT(x, y) PP_CAT_(x, y)
#endif

#ifdef USE_LAZY_TYPES
#define TRAIT(trait, ...) typename trait<__VA_ARGS__>::type
#else
#define TRAIT(trait, ...) PP_CAT(trait, _t)<__VA_ARGS__>
#endif

#ifdef BOOST_CLBL_TRTS_ENABLE_NOEXCEPT_TYPES
#define TEST_NOEXCEPT_QUAL noexcept
#else
#define TEST_NOEXCEPT_QUAL
#endif

#ifdef BOOST_CLBL_TRTS_DISABLE_ABOMINABLE_FUNCTIONS
#define TEST_ABOM_V
#else
#define TEST_ABOM_V volatile
#endif

template<typename T1, typename T2>
void assert_same() {
    CT_ASSERT(std::is_same<T1, T2>::value);
}