<commit_before>#include "image_geometry/stereo_camera_model.h"

namespace image_geometry {

StereoCameraModel::StereoCameraModel()
  : Q_(0.0)
{
  Q_(0,0) = Q_(1,1) = 1.0;
}

StereoCameraModel::StereoCameraModel(const StereoCameraModel& other)
  : left_(other.left_), right_(other.right_),
    Q_(0.0)
{
  Q_(0,0) = Q_(1,1) = 1.0;
  if (other.initialized())
    updateQ();
}

StereoCameraModel& StereoCameraModel::operator=(const StereoCameraModel& other)
{
  if (other.initialized())
    this->fromCameraInfo(other.left_.cameraInfo(), other.right_.cameraInfo());
  return *this;
}

bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
                                       const sensor_msgs::CameraInfo& right)
{
  bool changed_left  = left_.fromCameraInfo(left);
  bool changed_right = right_.fromCameraInfo(right);
  bool changed = changed_left || changed_right;

  // Note: don't require identical time stamps to allow imperfectly synced stereo.
  assert( left_.tfFrame() == right_.tfFrame() );
  assert( left_.fx() == right_.fx() );
  assert( left_.fy() == right_.fy() );
  assert( left_.cy() == right_.cy() );
  // cx may differ for verged cameras

  if (changed)
    updateQ();

  return changed;
}

bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
                                       const sensor_msgs::CameraInfoConstPtr& right)
{
  return fromCameraInfo(*left, *right);
}

void StereoCameraModel::updateQ()
{
  // Update variable fields of reprojection matrix
  /*
    From Springer Handbook of Robotics, p. 524:
        [ Fx 0  Cx -FxTx ]
    P = [ 0  Fy Cy   0   ]
        [ 0  0  1    0   ]

        [ 1 0   0      -Cx      ]
    Q = [ 0 1   0      -Cy      ]
        [ 0 0   0       Fx      ]
        [ 0 0 -1/Tx (Cx-Cx')/Tx ]
    where primed parameters are from the left projection matrix, unprimed from the right.

    Disparity = x_left - x_right
   */
  double Tx = baseline();
  Q_(3,2) = 1.0 / Tx;
  Q_(0,3) = -right_.cx();
  Q_(1,3) = -right_.cy();
  Q_(2,3) = right_.fx();
  Q_(3,3) = (right_.cx() - left_.cx()) / Tx; // zero when disparities are pre-adjusted
}

void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
                                             cv::Point3d& xyz) const
{
  assert( initialized() );

  // Do the math inline:
  // [X Y Z W]^T = Q * [u v d 1]^T
  // Point = (X/W, Y/W, Z/W)
  // cv::perspectiveTransform could be used but with more overhead.
  double u = left_uv_rect.x, v = left_uv_rect.y;
  cv::Point3d XYZ(u + Q_(0,3), v + Q_(1,3), Q_(2,3));
  double W = Q_(3,2)*disparity + Q_(3,3);
  xyz = XYZ * (1.0/W);
}

const double StereoCameraModel::MISSING_Z = 10000.;

void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
                                                  bool handleMissingValues) const
{
  assert( initialized() );

  cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
}

} //namespace image_geometry
<commit_msg>Updated Q matrix to account for cameras with different Fx and Fy values<commit_after>#include "image_geometry/stereo_camera_model.h"

namespace image_geometry {

StereoCameraModel::StereoCameraModel()
  : Q_(0.0)
{
  Q_(0,0) = Q_(1,1) = 1.0;
}

StereoCameraModel::StereoCameraModel(const StereoCameraModel& other)
  : left_(other.left_), right_(other.right_),
    Q_(0.0)
{
  Q_(0,0) = Q_(1,1) = 1.0;
  if (other.initialized())
    updateQ();
}

StereoCameraModel& StereoCameraModel::operator=(const StereoCameraModel& other)
{
  if (other.initialized())
    this->fromCameraInfo(other.left_.cameraInfo(), other.right_.cameraInfo());
  return *this;
}

bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
                                       const sensor_msgs::CameraInfo& right)
{
  bool changed_left  = left_.fromCameraInfo(left);
  bool changed_right = right_.fromCameraInfo(right);
  bool changed = changed_left || changed_right;

  // Note: don't require identical time stamps to allow imperfectly synced stereo.
  assert( left_.tfFrame() == right_.tfFrame() );
  assert( left_.fx() == right_.fx() );
  assert( left_.fy() == right_.fy() );
  assert( left_.cy() == right_.cy() );
  // cx may differ for verged cameras

  if (changed)
    updateQ();

  return changed;
}

bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
                                       const sensor_msgs::CameraInfoConstPtr& right)
{
  return fromCameraInfo(*left, *right);
}

void StereoCameraModel::updateQ()
{
  // Update variable fields of reprojection matrix
  /*
    From Springer Handbook of Robotics, p. 524:

         [ Fx    0  Cx   0   ]
    P  = [ 0     Fy Cy   0   ]
         [ 0     0  1    0   ]

         [ Fx    0  Cx' -FxTx ]
    P' = [ 0     Fy Cy   0    ]
         [ 0     0  1    0    ]
    where primed parameters are from the left projection matrix, unprimed from the right.

    [u   v 1]^T = P  * [x y z 1]^T
    [u-d v 1]^T = P' * [x y z 1]^T

    Combining the two equations above results in the following equation

    [u v u-d 1]^T = [ Fx   0    Cx   0    ] * [ x y z 1]^T
                    [ 0    Fy   Cy   0    ]
                    [ Fx   0    Cx'  FxTx ]
                    [ 0    0    1    0    ]

    Subtracting the 3rd from from the first and inverting the expression
    results in the following equation.

    [x y z 1]^T = Q * [u v d 1]^T

    Where Q is defined as

    Q = [ FyTx  0     0   -FyCxTx     ]
        [ 0     FxTx  0   -FxCyTx     ]
        [ 0     0     0    FxFyTx     ]
        [ 0     0     -Fy  Fy(Cx-Cx') ]

   Using the assumption Fx = Fy Q can be simplified to the following. But for
   compatibility with stereo cameras with different focal lengths we will use
   the full Q matrix.

        [ 1 0   0      -Cx      ]
    Q = [ 0 1   0      -Cy      ]
        [ 0 0   0       Fx      ]
        [ 0 0 -1/Tx (Cx-Cx')/Tx ]

    Disparity = x_left - x_right

    For compatibility with stereo cameras with different focal lengths we will use
    the full Q matrix.

   */
  double Tx = -baseline(); // The baseline member negates our Tx. Undo this negation
  Q_(0,0) =  left_.fy() * Tx;
  Q_(0,3) = -left_.fy() * left_.cx() * Tx;
  Q_(1,1) =  left_.fx() * Tx;
  Q_(1,3) = -left_.fx() * left_.cy() * Tx;
  Q_(2,3) =  left_.fx() * left_.fy() * Tx;
  Q_(3,2) = -left_.fy();
  Q_(3,3) =  left_.fy() * (left_.cx() - right_.cx()); // zero when disparities are pre-adjusted
}

void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
                                             cv::Point3d& xyz) const
{
  assert( initialized() );

  // Do the math inline:
  // [X Y Z W]^T = Q * [u v d 1]^T
  // Point = (X/W, Y/W, Z/W)
  // cv::perspectiveTransform could be used but with more overhead.
  double u = left_uv_rect.x, v = left_uv_rect.y;
  cv::Point3d XYZ(u + Q_(0,3), v + Q_(1,3), Q_(2,3));
  double W = Q_(3,2)*disparity + Q_(3,3);
  xyz = XYZ * (1.0/W);
}

const double StereoCameraModel::MISSING_Z = 10000.;

void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
                                                  bool handleMissingValues) const
{
  assert( initialized() );

  cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
}

} //namespace image_geometry
<|endoftext|>


<commit_before>// Copyright (c) 2017 offa
// Copyright 2011 Ciaran McHale.
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions.
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "danek/Configuration.h"
#include "danek/SchemaValidator.h"
#include "danek/PatternMatch.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <locale.h>

using namespace danek;

static void parseCmdLineArgs(int argc, char** argv, const char*& cfgFile, bool& wantDiagnostics);
static void usage();

int main(int argc, char** argv)
{
    Configuration* cfg = Configuration::create();
    const char* cfgFile;
    const char* exPattern = nullptr;
    bool wantDiagnostics;
    StringBuffer buf;
    StringVector testSchema;
    StringVector goodScopes;
    StringVector badScopes;
    SchemaValidator fileSv;
    SchemaValidator testSv;
    std::size_t passedCount = 0;

    setlocale(LC_ALL, "");
    parseCmdLineArgs(argc, argv, cfgFile, wantDiagnostics);

    //--------
    // Parse and validate the configuration file. Then get the testSchema
    // and lists of the names of good and bad sub-scopes.
    //--------
    try
    {
        const char* fileSchema[] = {
            "testSchema = list[string]",
            "good = scope",
            "bad = scope",
            "@ignoreScopesIn good",
            "@ignoreScopesIn bad",
            0 // null-terminated array
        };
        cfg->parse(cfgFile);
        fileSv.parseSchema(fileSchema);
        fileSv.validate(cfg, "", "");

        cfg->lookupList("testSchema", "", testSchema);
        cfg->listFullyScopedNames("good", "", ConfType::Scope, false, goodScopes);
        cfg->listFullyScopedNames("bad", "", ConfType::Scope, false, badScopes);
        testSv.wantDiagnostics(wantDiagnostics);

        // Deprecated conversion
        std::vector<const char*> v;
        v.reserve(testSchema.size());

        for( const auto& s : testSchema.get() )
        {
            v.push_back(s.c_str());
        }

        testSv.parseSchema(v.data());
    }
    catch (const ConfigurationException& ex)
    {
        printf("%s\n", ex.message().c_str());
        return 1;
    }

    //--------
    // Schema validation should succeed for every sub-scope withinin "good".
    //--------
    for ( std::size_t i = 0; i < goodScopes.size(); i++)
    {
        try
        {
            testSv.validate(cfg, goodScopes[i].c_str(), "");
            passedCount++;
        }
        catch (const ConfigurationException& ex)
        {
            try
            {
                cfg->dump(buf, true, goodScopes[i].c_str(), "");
            }
            catch (const ConfigurationException& ex2)
            {
                throw; // Bug!
            }
            printf("\n\n--------\n");
            printf("%s\n\n%s--------\n\n", ex.message().c_str(), buf.str().c_str());
        }
    }

    //--------
    // Schema validation should fail for every sub-scope within "bad".
    //--------
    for ( std::size_t i = 0; i < badScopes.size(); i++)
    {
        try
        {
            exPattern = cfg->lookupString(badScopes[i].c_str(), "exception");
            testSv.validate(cfg, badScopes[i].c_str(), "");
            try
            {
                cfg->dump(buf, true, badScopes[i].c_str(), "");
            }
            catch (const ConfigurationException& ex2)
            {
                throw; // Bug!
            }
            printf("\n\n--------\n");
            printf("Validation succeeded for scope '%s'\n%s--------\n\n", badScopes[i].c_str(), buf.str().c_str());
        }
        catch (const ConfigurationException& ex)
        {
            if (patternMatch(ex.message().c_str(), exPattern))
            {
                ++passedCount;
            }
            else
            {
                printf("\n\n--------\n");
                printf("Unexpected exception for scope \"%s\"\n"
                       "Pattern \"%s\" does not match "
                       "exception\n\"%s\"\n--------\n\n",
                    badScopes[i].c_str(),
                    exPattern,
                    ex.message().c_str());
            }
        }
    }

    const auto totalCount = goodScopes.size() + badScopes.size();
    printf("\n%d tests out of %d passed\n\n", static_cast<int>(passedCount), static_cast<int>(totalCount));

    cfg->destroy();
    return static_cast<int>(passedCount != totalCount);
}

static void parseCmdLineArgs(int argc, char** argv, const char*& cfgFile, bool& wantDiagnostics)
{
    int i;

    cfgFile = 0;
    wantDiagnostics = false;

    for (i = 1; i < argc; i++)
    {
        if (strcmp(argv[i], "-h") == 0)
        {
            usage();
        }
        else if (strcmp(argv[i], "-diagnostics") == 0)
        {
            wantDiagnostics = true;
        }
        else if (strcmp(argv[i], "-nodiagnostics") == 0)
        {
            wantDiagnostics = false;
        }
        else if (strcmp(argv[i], "-cfg") == 0)
        {
            if (i == argc - 1)
            {
                usage();
            }
            cfgFile = argv[i + 1];
            i++;
        }
        else
        {
            fprintf(stderr, "\nUnrecognised option '%s'\n", argv[i]);
            usage();
        }
    }
    if (cfgFile == 0)
    {
        fprintf(stderr, "\nYou must specify '-cfg <file.cfg>'\n");
        usage();
    }
}

static void usage()
{
    fprintf(stderr,
        "\n"
        "usage: schema-testsuite <options>\n"
        "\n"
        "The <options> can be:\n"
        "  -h               Print this usage statement\n"
        "  -cfg <file.cfg>  Parse specified configuration file\n"
        "  -diagnostics     Prints diagnostics from schema validator\n"
        "  -nodiagnostics   No diagnostics (default)\n"
        "\n");
    exit(1);
}
<commit_msg>Some improvements.<commit_after>// Copyright (c) 2017 offa
// Copyright 2011 Ciaran McHale.
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions.
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "danek/Configuration.h"
#include "danek/SchemaValidator.h"
#include "danek/PatternMatch.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <locale.h>

using namespace danek;

static void parseCmdLineArgs(int argc, char** argv, const char*& cfgFile, bool& wantDiagnostics);
static void usage();

int main(int argc, char** argv)
{
    Configuration* cfg = Configuration::create();
    const char* cfgFile;
    const char* exPattern = nullptr;
    bool wantDiagnostics;
    StringBuffer buf;
    StringVector testSchema;
    StringVector goodScopes;
    StringVector badScopes;
    SchemaValidator fileSv;
    SchemaValidator testSv;
    std::size_t passedCount = 0;

    setlocale(LC_ALL, "");
    parseCmdLineArgs(argc, argv, cfgFile, wantDiagnostics);

    //--------
    // Parse and validate the configuration file. Then get the testSchema
    // and lists of the names of good and bad sub-scopes.
    //--------
    try
    {
        const char* fileSchema[] = {
            "testSchema = list[string]",
            "good = scope",
            "bad = scope",
            "@ignoreScopesIn good",
            "@ignoreScopesIn bad",
            0 // null-terminated array
        };
        cfg->parse(cfgFile);
        fileSv.parseSchema(fileSchema);
        fileSv.validate(cfg, "", "");

        cfg->lookupList("testSchema", "", testSchema);
        cfg->listFullyScopedNames("good", "", ConfType::Scope, false, goodScopes);
        cfg->listFullyScopedNames("bad", "", ConfType::Scope, false, badScopes);
        testSv.wantDiagnostics(wantDiagnostics);

        // Deprecated conversion
        std::vector<const char*> v;
        v.reserve(testSchema.size());

        for( const auto& s : testSchema.get() )
        {
            v.push_back(s.c_str());
        }

        testSv.parseSchema(v.data());
    }
    catch (const ConfigurationException& ex)
    {
        printf("%s\n", ex.message().c_str());
        return 1;
    }

    //--------
    // Schema validation should succeed for every sub-scope withinin "good".
    //--------
    for ( std::size_t i = 0; i < goodScopes.size(); ++i)
    {
        try
        {
            testSv.validate(cfg, goodScopes[i].c_str(), "");
            passedCount++;
        }
        catch (const ConfigurationException& ex)
        {
            try
            {
                cfg->dump(buf, true, goodScopes[i].c_str(), "");
            }
            catch (const ConfigurationException& ex2)
            {
                throw; // Bug!
            }
            printf("\n\n--------\n");
            printf("%s\n\n%s--------\n\n", ex.message().c_str(), buf.str().c_str());
        }
    }

    //--------
    // Schema validation should fail for every sub-scope within "bad".
    //--------
    for ( std::size_t i = 0; i < badScopes.size(); ++i)
    {
        try
        {
            exPattern = cfg->lookupString(badScopes[i].c_str(), "exception");
            testSv.validate(cfg, badScopes[i].c_str(), "");
            try
            {
                cfg->dump(buf, true, badScopes[i].c_str(), "");
            }
            catch (const ConfigurationException& ex2)
            {
                throw; // Bug!
            }
            printf("\n\n--------\n");
            printf("Validation succeeded for scope '%s'\n%s--------\n\n", badScopes[i].c_str(), buf.str().c_str());
        }
        catch (const ConfigurationException& ex)
        {
            if (patternMatch(ex.message().c_str(), exPattern))
            {
                ++passedCount;
            }
            else
            {
                printf("\n\n--------\n");
                printf("Unexpected exception for scope \"%s\"\n"
                       "Pattern \"%s\" does not match "
                       "exception\n\"%s\"\n--------\n\n",
                    badScopes[i].c_str(),
                    exPattern,
                    ex.message().c_str());
            }
        }
    }

    const auto totalCount = goodScopes.size() + badScopes.size();
    printf("\n%d tests out of %d passed\n\n", static_cast<int>(passedCount), static_cast<int>(totalCount));

    cfg->destroy();
    return static_cast<int>(passedCount != totalCount);
}

static void parseCmdLineArgs(int argc, char** argv, const char*& cfgFile, bool& wantDiagnostics)
{
    cfgFile = 0;
    wantDiagnostics = false;

    for (int i = 1; i < argc; ++i)
    {
        if (strcmp(argv[i], "-h") == 0)
        {
            usage();
        }
        else if (strcmp(argv[i], "-diagnostics") == 0)
        {
            wantDiagnostics = true;
        }
        else if (strcmp(argv[i], "-nodiagnostics") == 0)
        {
            wantDiagnostics = false;
        }
        else if (strcmp(argv[i], "-cfg") == 0)
        {
            if (i == argc - 1)
            {
                usage();
            }
            cfgFile = argv[i + 1];
            ++i;
        }
        else
        {
            fprintf(stderr, "\nUnrecognised option '%s'\n", argv[i]);
            usage();
        }
    }
    if (cfgFile == 0)
    {
        fprintf(stderr, "\nYou must specify '-cfg <file.cfg>'\n");
        usage();
    }
}

static void usage()
{
    fprintf(stderr,
        "\n"
        "usage: schema-testsuite <options>\n"
        "\n"
        "The <options> can be:\n"
        "  -h               Print this usage statement\n"
        "  -cfg <file.cfg>  Parse specified configuration file\n"
        "  -diagnostics     Prints diagnostics from schema validator\n"
        "  -nodiagnostics   No diagnostics (default)\n"
        "\n");
    exit(1);
}
<|endoftext|>


<commit_before>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file adaptive.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__UTIL__META__OPERATOR__ADAPTIVE_HPP
#define FL__UTIL__META__OPERATOR__ADAPTIVE_HPP

#include <fl/util/meta/size_deduction.hpp>
#include <fl/util/meta/operator/not_adaptive.hpp>
#include <fl/model/adaptive_model.hpp>
#include <fl/model/process/process_model_interface.hpp>
#include <fl/model/observation/observation_model_interface.hpp>

namespace fl
{

/**
 * \ingroup meta
 *
 * Adaptive operator
 */
template <typename... Model> class Adaptive;

/**
 * \internal
 * \ingroup meta
 *
 * Adaptive operator specialization applied on the passed model type
 */
template <typename Model>
class Adaptive<Model>
  : public Adaptive<typename Model::ModelType, Model>
{ };

/**
 * \internal
 * \ingroup meta
 */
template <typename Model>
class Adaptive<NotAdaptive<Model>>
  : public Adaptive<
        typename Model::ModelType,
        NotAdaptive<typename Model::ModelType, Model>
    >
{ };

/**
 * \ingroup meta
 */
template <typename Model>
struct Traits<Adaptive<internal::ObsrvModelType, Model>>
    : Traits<Model>
{
    typedef typename Traits<Model>::State ActualState;

    typedef Eigen::Matrix<
                typename Traits<Model>::Scalar,
                JoinSizes<
                    ActualState::SizeAtCompileTime,
                    Traits<Model>::Param::SizeAtCompileTime
                >::Size,
                1
            > State;
};

/**
 * Adaptive operator implementation of the observation model
 */
template <typename Model>
class Adaptive<internal::ObsrvModelType, Model>
  : public Model
{
public:
    typedef typename Traits<Model>::Obsrv Obsrv;
    typedef typename Traits<Model>::State State;
    typedef typename Traits<Model>::Noise Noise;
    typedef typename Traits<Model>::Param Param;

    /**
     * Evaluates the model function \f$y = h(x, w)\f$ where \f$x\f$ is the state
     * and \f$w\sim {\cal N}(0, 1)\f$ is a white noise parameter. Put
     * differently, \f$y = h(x, w)\f$ is a sample from the conditional model
     * distribution \f$p(y \mid x)\f$.
     *
     * \param state         The state variable \f$x\f$
     * \param noise         The noise term \f$w\f$
     * \param delta_time    Prediction time
     */
    virtual Obsrv predict_obsrv(const State& state,
                                const Noise& noise,
                                double delta_time)
    {
        assert(state.rows() ==
               Model::param_dimension() + Model::state_dimension());

        Model::param(state.bottomRows(Model::param_dimension()));

        return Model::predict_obsrv(state.topRows(Model::state_dimension()),
                                    noise, delta_time);
    }

    virtual int state_dimension() const
    {
        return Model::state_dimension() + Model::param_dimension();
    }
};

}

#endif
<commit_msg>Added default Adaptive implementation<commit_after>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file adaptive.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__UTIL__META__OPERATOR__ADAPTIVE_HPP
#define FL__UTIL__META__OPERATOR__ADAPTIVE_HPP

#include <fl/util/meta/size_deduction.hpp>
#include <fl/util/meta/operator/not_adaptive.hpp>
#include <fl/model/adaptive_model.hpp>
#include <fl/model/process/process_model_interface.hpp>
#include <fl/model/observation/observation_model_interface.hpp>

namespace fl
{

/**
 * \ingroup meta
 *
 * Adaptive operator
 */
template <typename... Model>
class Adaptive
{ };

/**
 * \internal
 * \ingroup meta
 *
 * Adaptive operator specialization applied on the passed model type
 */
template <typename Model>
class Adaptive<Model>
  : public Adaptive<typename Model::ModelType, Model>
{ };

/**
 * \internal
 * \ingroup meta
 */
template <typename Model>
class Adaptive<NotAdaptive<Model>>
  : public Adaptive<
        typename Model::ModelType,
        NotAdaptive<typename Model::ModelType, Model>
    >
{ };

/**
 * \ingroup meta
 */
template <typename Model>
struct Traits<Adaptive<internal::ObsrvModelType, Model>>
    : Traits<Model>
{
    typedef typename Traits<Model>::State ActualState;

    typedef Eigen::Matrix<
                typename Traits<Model>::Scalar,
                JoinSizes<
                    ActualState::SizeAtCompileTime,
                    Traits<Model>::Param::SizeAtCompileTime
                >::Size,
                1
            > State;
};

/**
 * Adaptive operator implementation of the observation model
 */
template <typename Model>
class Adaptive<internal::ObsrvModelType, Model>
  : public Model
{
public:
    typedef typename Traits<Model>::Obsrv Obsrv;
    typedef typename Traits<Model>::State State;
    typedef typename Traits<Model>::Noise Noise;
    typedef typename Traits<Model>::Param Param;

    /**
     * Evaluates the model function \f$y = h(x, w)\f$ where \f$x\f$ is the state
     * and \f$w\sim {\cal N}(0, 1)\f$ is a white noise parameter. Put
     * differently, \f$y = h(x, w)\f$ is a sample from the conditional model
     * distribution \f$p(y \mid x)\f$.
     *
     * \param state         The state variable \f$x\f$
     * \param noise         The noise term \f$w\f$
     * \param delta_time    Prediction time
     */
    virtual Obsrv predict_obsrv(const State& state,
                                const Noise& noise,
                                double delta_time)
    {
        assert(state.rows() ==
               Model::param_dimension() + Model::state_dimension());

        Model::param(state.bottomRows(Model::param_dimension()));

        return Model::predict_obsrv(state.topRows(Model::state_dimension()),
                                    noise, delta_time);
    }

    virtual int state_dimension() const
    {
        return Model::state_dimension() + Model::param_dimension();
    }
};

}

#endif
<|endoftext|>


<commit_before>#pragma once
#include <sstream>
#include <imgui.h>

namespace GUI {
    class LogView {
        public:
        LogView();
        ~LogView();

        void Show();

        bool IsVisible();

        private:
        std::stringstream defaultStringstream;
        std::stringstream infoStringstream;
        std::stringstream warningStringstream;
        std::stringstream errorStringstream;

        ImGuiTextBuffer textBuffer;
        ImGuiTextFilter textFilter;
        ImVector<int> lineOffsets;

        bool visible = true;

        static const int splitterSize = 2;
        int logHeight = 250;
        bool logResize = false;

        int sceneWidth = 250;
        bool sceneResize = false;

        int editorWidth = 250;
        bool editorResize = false;
    };
}<commit_msg>Doxygen comments.<commit_after>#pragma once
#include <sstream>
#include <imgui.h>

namespace GUI {
    class LogView {
        public:
        /// Constructor.
        LogView();

        /// Destructor.
        ~LogView();

        /// Shows the log.
        void Show();

        /// If the log is visible or not.
        /**
         * @return Whether the log is visible or not.
         */
        bool IsVisible();

        private:
        std::stringstream defaultStringstream;
        std::stringstream infoStringstream;
        std::stringstream warningStringstream;
        std::stringstream errorStringstream;

        ImGuiTextBuffer textBuffer;
        ImGuiTextFilter textFilter;
        ImVector<int> lineOffsets;

        bool visible = true;

        static const int splitterSize = 2;
        int logHeight = 250;
        bool logResize = false;

        int sceneWidth = 250;
        bool sceneResize = false;

        int editorWidth = 250;
        bool editorResize = false;
    };
}<|endoftext|>


<commit_before>/*
    TODO:
        - Resolver o sistema encontrado no final.
        - Buscar exemplos e testar desempenho.
            - Procurar ferramentas para medição de desempenho de programas paralelos.
        - Gerar malha com o resultado final.
        - Refatorar código.
            - Modularizar os trechos de código (organizar em funções e classes).
*/

#include "moab/Core.hpp"
#include "moab/MeshTopoUtil.hpp"
#ifdef MOAB_HAVE_MPI
#include "moab/ParallelComm.hpp"
#include "MBParallelConventions.h"
#endif
#include <Epetra_MpiComm.h>
#include <Epetra_CrsMatrix.h>
#include <Epetra_Map.h>
#include <Epetra_Vector.h>
#include <Epetra_Version.h>
#include <AztecOO.h>
#include <mpi.h>
#include <iostream>
#include <string>
#include <set>
#include <numeric>
#include <cstdlib>
#include <cstdio>
#include <cmath>

#define ALL_PROCS -1
#define ALL_DIM -1
#define GHOST_DIM 3
#define BRIDGE_DIM 2

using namespace std;
using namespace moab;

double get_equivalent_perm (double k1[9], double k2[9], double unit_vector[3]) {
    double k1_pre[3] = {k1[0]*unit_vector[0], k1[4]*unit_vector[1], k1[8]*unit_vector[2]};
    double k2_pre[3] = {k2[0]*unit_vector[0], k2[4]*unit_vector[1], k2[8]*unit_vector[2]};
    double k1_eq = k1_pre[0]*unit_vector[0] + k1_pre[1]*unit_vector[1] + k1_pre[2]*unit_vector[2];
    double k2_eq = k2_pre[0]*unit_vector[0] + k2_pre[1]*unit_vector[1] + k2_pre[2]*unit_vector[2];
    return 2*k1_eq*k2_eq/(k1_eq + k2_eq);
}

double get_centroid_dist (double c1[], double c2[]) {
    return sqrt(pow(c1[0] + c2[0], 2) + pow(c1[1] + c2[1], 2) + pow(c1[2] + c2[2], 2));
}

void get_unit_vector (double c1[], double c2[], double dist, double *u) {
    u[0] = (c2[0] - c1[0])/dist;
    u[1] = (c2[1] - c1[1])/dist;
    u[2] = (c2[2] - c1[2])/dist;
}

int main(int argc, char **argv) {
#ifndef MOAB_HAVE_MPI
  cout << "Run with mpiexec or mpirun" << endl;
  return -1;
#endif
    MPI_Init(&argc, &argv);

    // Options for file read.
    string input_file = "part_mesh.h5m";
    string output_file = "solve_mesh.h5m";
    string parallel_read_opts = "PARALLEL=READ_PART;PARTITION=PARALLEL_PARTITION;PARALLEL_RESOLVE_SHARED_ENTS";
    ErrorCode rval;

    // In case the user provides another file name, use it.
    if (argc > 2) {
        input_file = string(argv[1]);
    }

    // Creating instances of moab::Core and moab::ParallelComm
    Interface* mb = new Core;
    if (mb == NULL) {
        cout << "Unable to create MOAB interface." << endl;
        return -1;
    }

    ParallelComm* pcomm = new ParallelComm(mb, MPI_COMM_WORLD);
    int rank, world_size;
    if (pcomm == NULL) {
        cout << "Unable to create ParallelComm instance." << endl;
        return -1;
    }
    else {
        // Getting processor rank and the total number of processes on communicator.
        rank = pcomm->proc_config().proc_rank();
        world_size = pcomm->proc_config().proc_size();
    }

    // Open file containing the mesh with the options specified above.
    rval = mb->load_file(input_file.c_str(), 0, parallel_read_opts.c_str());
    MB_CHK_SET_ERR(rval, "load_file failed");

    // Creating an instance of moab::MeshTopoUtil for handling operations with
    // elements adjacencies.
    MeshTopoUtil* topo_util = new MeshTopoUtil(mb);
    if (topo_util == NULL) {
        cout << "Failed to create MeshTopoUtil instance." << endl;
        return -1;
    }

    // Exchange one layer of ghost elements, i.e., get the neighbors elements
    // that belong to another partition.
    Range my_elems;
    rval = mb->get_entities_by_dimension(0, 3, my_elems, false); MB_CHK_ERR(rval);
    rval = pcomm->exchange_ghost_cells(GHOST_DIM, BRIDGE_DIM, 1, 0, true);
    MB_CHK_SET_ERR(rval, "exchange_ghost_cells failed");

    // Creating the Epetra structures (Epetra_map and Epetra_comm)
    Epetra_MpiComm epetra_comm (MPI_COMM_WORLD);

    // Computes the number of elements in the mesh.
    int num_local_elems = my_elems.size(), num_global_elems = 0;
    MPI_Allreduce(&num_local_elems, &num_global_elems, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

    // Gets the global ID for each element in the partition.
    int* gids = (int*) calloc(my_elems.size(), sizeof(int));
    if (gids == NULL) {
        cout << "Null pointer" << endl;
        return -1;
    }
    Tag global_id_tag, centroid_tag, perm_tag, dirichlet_tag;
    rval = mb->tag_get_handle("GLOBAL_ID", global_id_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("CENTROID", centroid_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("PERMEABILITY", perm_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("DIRICHLET_BC", dirichlet_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_data(global_id_tag, my_elems, (void*) gids); MB_CHK_ERR(rval);

    // It is necessary to exchange tags for the ghost elements. Those are not
    // transferred when exchange_ghost_cells is called.
    Range empty_set;
    rval = pcomm->exchange_tags(centroid_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for centroid failed");
    rval = pcomm->exchange_tags(perm_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for permeability failed");
    rval = pcomm->exchange_tags(dirichlet_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for dirichlet bc failed");

    // Initializing the Epetra structure for a sparse matrix.
    Epetra_Map row_map (num_global_elems, num_local_elems, gids, 0, epetra_comm);
    Epetra_CrsMatrix A (Copy, row_map, 6);   // Sparse matrix w/ at most 6 entries per line.
    Epetra_Vector B (row_map);
    Epetra_Vector X (row_map);

    // The code block above does the actual calculation for the method.
    Range adjacencies;
    std::vector<double> row_values;
    std::vector<int> row_indexes;
    double e1_centroid[3], e2_centroid[3], e1_perm[9], e2_perm[9], pressure = 0, diag_coef = 0;
    double equiv_perm = 0.0, centroid_dist = 0.0;
    double unit_vector[3] = {0, 0, 0};
    int row_id = -1, n = 0;
    printf("<%d> Computing the method\n", rank);
    for (Range::iterator it = my_elems.begin(); it != my_elems.end(); it++) {
        rval = mb->tag_get_data(dirichlet_tag, &(*it), 1, &pressure); MB_CHK_ERR(rval);
        if (pressure == 0) {
            rval = topo_util->get_bridge_adjacencies(*it, 2, 3, adjacencies); MB_CHK_ERR(rval);
            rval = mb->tag_get_data(centroid_tag, &(*it), 1, &e1_centroid); MB_CHK_ERR(rval);
            rval = mb->tag_get_data(perm_tag, &(*it), 1, &e1_perm); MB_CHK_ERR(rval);
            for (Range::iterator itt = adjacencies.begin(); itt != adjacencies.end(); itt++) {
                rval = mb->tag_get_data(centroid_tag, &(*itt), 1, &e2_centroid); MB_CHK_ERR(rval);
                rval = mb->tag_get_data(perm_tag, &(*itt), 1, &e2_perm); MB_CHK_ERR(rval);
                rval = mb->tag_get_data(global_id_tag, &(*itt), 1, &row_id); MB_CHK_ERR(rval);

                centroid_dist = get_centroid_dist(e1_centroid, e2_centroid);

                get_unit_vector(e1_centroid, e2_centroid, centroid_dist, &unit_vector[0]);
                equiv_perm = get_equivalent_perm(e1_perm, e2_perm, unit_vector);

                row_values.push_back(-equiv_perm/pow(centroid_dist, 2));
                row_indexes.push_back(row_id);
            }
            diag_coef = (-1)*accumulate(row_values.begin(), row_values.end(), 0.0);
        }
        else {
            diag_coef = 1;
        }
        rval = mb->tag_get_data(global_id_tag, &(*it), 1, &row_id); MB_CHK_ERR(rval);

        row_values.push_back(diag_coef);
        row_indexes.push_back(row_id);

        A.InsertGlobalValues(row_id, row_values.size(), &row_values[0], &row_indexes[0]);
        B[n++] = pressure;

        row_values.clear();
        row_indexes.clear();
        adjacencies.clear();
    }
    printf("<%d> Done.\n", rank);
    A.FillComplete();
    MPI_Barrier(MPI_COMM_WORLD);

    Epetra_LinearProblem linear_problem (&A, &X, &B);
    AztecOO solver (linear_problem);
    solver.Iterate(100000, 1e-10);
    printf("<%d> Solved it.\n", rank);

    printf("<%d> Setting pressure tag\n", rank);
    Tag pressure_tag;
    rval = mb->tag_get_handle("PRESSURE", 1, MB_TYPE_DOUBLE, pressure_tag, MB_TAG_DENSE);
    MB_CHK_SET_ERR(rval, "tag_get_handle to pressure_tag failed");
    rval = mb->tag_set_data(pressure_tag, my_elems, &X[0]);
    printf("<%d> Done\n", rank);

    // Cleaning up alocated objects.
    delete topo_util;
    delete pcomm;
    delete mb;
    MPI_Finalize();

    return 0;
}
<commit_msg>Add files via upload<commit_after>/*
    TODO:
        - Resolver o sistema encontrado no final.
        - Buscar exemplos e testar desempenho.
            - Procurar ferramentas para medição de desempenho de programas paralelos.
        - Gerar malha com o resultado final.
        - Refatorar código.
            - Modularizar os trechos de código (organizar em funções e classes).
*/

#include "moab/Core.hpp"
#include "moab/MeshTopoUtil.hpp"
#ifdef MOAB_HAVE_MPI
#include "moab/ParallelComm.hpp"
#include "MBParallelConventions.h"
#endif
#include <Epetra_MpiComm.h>
#include <Epetra_CrsMatrix.h>
#include <Epetra_Map.h>
#include <Epetra_Vector.h>
#include <Epetra_Version.h>
#include <AztecOO.h>
#include <mpi.h>
#include <iostream>
#include <string>
#include <set>
#include <numeric>
#include <cstdlib>
#include <cstdio>
#include <cmath>

#define ALL_PROCS -1
#define ALL_DIM -1
#define GHOST_DIM 3
#define BRIDGE_DIM 2

using namespace std;
using namespace moab;

double get_equivalent_perm (double k1[9], double k2[9], double unit_vector[3]) {
    double k1_pre[3] = {k1[0]*unit_vector[0], k1[4]*unit_vector[1], k1[8]*unit_vector[2]};
    double k2_pre[3] = {k2[0]*unit_vector[0], k2[4]*unit_vector[1], k2[8]*unit_vector[2]};
    double k1_eq = k1_pre[0]*unit_vector[0] + k1_pre[1]*unit_vector[1] + k1_pre[2]*unit_vector[2];
    double k2_eq = k2_pre[0]*unit_vector[0] + k2_pre[1]*unit_vector[1] + k2_pre[2]*unit_vector[2];
    return 2*k1_eq*k2_eq/(k1_eq + k2_eq);
}

double get_centroid_dist (double c1[], double c2[]) {
    return sqrt(pow(c1[0] + c2[0], 2) + pow(c1[1] + c2[1], 2) + pow(c1[2] + c2[2], 2));
}

void get_unit_vector (double c1[], double c2[], double dist, double *u) {
    u[0] = (c2[0] - c1[0])/dist;
    u[1] = (c2[1] - c1[1])/dist;
    u[2] = (c2[2] - c1[2])/dist;
}

int main(int argc, char **argv) {
#ifndef MOAB_HAVE_MPI
  cout << "Run with mpiexec or mpirun" << endl;
  return -1;
#endif
    MPI_Init(&argc, &argv);

    // Options for file read.
    string input_file = "part_mesh.h5m";
    string output_file = "solve_mesh.h5m";
    string parallel_read_opts = "PARALLEL=READ_PART;PARTITION=PARALLEL_PARTITION;PARALLEL_RESOLVE_SHARED_ENTS";
    ErrorCode rval;

    // In case the user provides another file name, use it.
    if (argc > 2) {
        input_file = string(argv[1]);
    }

    // Creating instances of moab::Core and moab::ParallelComm
    Interface* mb = new Core;
    if (mb == NULL) {
        cout << "Unable to create MOAB interface." << endl;
        return -1;
    }

    ParallelComm* pcomm = new ParallelComm(mb, MPI_COMM_WORLD);
    int rank, world_size;
    if (pcomm == NULL) {
        cout << "Unable to create ParallelComm instance." << endl;
        return -1;
    }
    else {
        // Getting processor rank and the total number of processes on communicator.
        rank = pcomm->proc_config().proc_rank();
        world_size = pcomm->proc_config().proc_size();
    }

    // Open file containing the mesh with the options specified above.
    rval = mb->load_file(input_file.c_str(), 0, parallel_read_opts.c_str());
    MB_CHK_SET_ERR(rval, "load_file failed");

    // Creating an instance of moab::MeshTopoUtil for handling operations with
    // elements adjacencies.
    MeshTopoUtil* topo_util = new MeshTopoUtil(mb);
    if (topo_util == NULL) {
        cout << "Failed to create MeshTopoUtil instance." << endl;
        return -1;
    }

    // Exchange one layer of ghost elements, i.e., get the neighbors elements
    // that belong to another partition.
    Range my_elems;
    rval = mb->get_entities_by_dimension(0, 3, my_elems, false); MB_CHK_ERR(rval);
    rval = pcomm->exchange_ghost_cells(GHOST_DIM, BRIDGE_DIM, 1, 0, true);
    MB_CHK_SET_ERR(rval, "exchange_ghost_cells failed");

    // Creating the Epetra structures (Epetra_map and Epetra_comm)
    Epetra_MpiComm epetra_comm (MPI_COMM_WORLD);

    // Computes the number of elements in the mesh.
    int num_local_elems = my_elems.size(), num_global_elems = 0;
    MPI_Allreduce(&num_local_elems, &num_global_elems, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

    // Gets the global ID for each element in the partition.
    int* gids = (int*) calloc(my_elems.size(), sizeof(int));
    if (gids == NULL) {
        cout << "Null pointer" << endl;
        return -1;
    }
    Tag global_id_tag, centroid_tag, perm_tag, dirichlet_tag;
    rval = mb->tag_get_handle("GLOBAL_ID", global_id_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("CENTROID", centroid_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("PERMEABILITY", perm_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_handle("DIRICHLET_BC", dirichlet_tag); MB_CHK_ERR(rval);
    rval = mb->tag_get_data(global_id_tag, my_elems, (void*) gids); MB_CHK_ERR(rval);

    // It is necessary to exchange tags for the ghost elements. Those are not
    // transferred when exchange_ghost_cells is called.
    Range empty_set;
    rval = pcomm->exchange_tags(centroid_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for centroid failed");
    rval = pcomm->exchange_tags(perm_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for permeability failed");
    rval = pcomm->exchange_tags(dirichlet_tag, empty_set);
    MB_CHK_SET_ERR(rval, "exchange_tags for dirichlet bc failed");

    // Initializing the Epetra structure for a sparse matrix.
    Epetra_Map row_map (num_global_elems, num_local_elems, gids, 0, epetra_comm);
    Epetra_CrsMatrix A (Copy, row_map, 6);   // Sparse matrix w/ at most 6 entries per line.
    Epetra_Vector B (row_map);
    Epetra_Vector X (row_map);

    // The code block above does the actual calculation for the method.
    Range adjacencies;
    std::vector<double> row_values;
    std::vector<int> row_indexes;
    double e1_centroid[3], e2_centroid[3], e1_perm[9], e2_perm[9], pressure = 0, diag_coef = 0;
    double equiv_perm = 0.0, centroid_dist = 0.0;
    double unit_vector[3] = {0, 0, 0};
    int row_id = -1, n = 0;
    printf("<%d> Computing the method\n", rank);
    for (Range::iterator it = my_elems.begin(); it != my_elems.end(); it++) {
        rval = mb->tag_get_data(dirichlet_tag, &(*it), 1, &pressure); MB_CHK_ERR(rval);
        if (pressure == 0) {
            rval = topo_util->get_bridge_adjacencies(*it, 2, 3, adjacencies); MB_CHK_ERR(rval);
            rval = mb->tag_get_data(centroid_tag, &(*it), 1, &e1_centroid); MB_CHK_ERR(rval);
            rval = mb->tag_get_data(perm_tag, &(*it), 1, &e1_perm); MB_CHK_ERR(rval);
            for (Range::iterator itt = adjacencies.begin(); itt != adjacencies.end(); itt++) {
                rval = mb->tag_get_data(centroid_tag, &(*itt), 1, &e2_centroid); MB_CHK_ERR(rval);
                rval = mb->tag_get_data(perm_tag, &(*itt), 1, &e2_perm); MB_CHK_ERR(rval);
                rval = mb->tag_get_data(global_id_tag, &(*itt), 1, &row_id); MB_CHK_ERR(rval);

                centroid_dist = get_centroid_dist(e1_centroid, e2_centroid);

                get_unit_vector(e1_centroid, e2_centroid, centroid_dist, &unit_vector[0]);
                equiv_perm = get_equivalent_perm(e1_perm, e2_perm, unit_vector);

                row_values.push_back(-equiv_perm/pow(centroid_dist, 2));
                row_indexes.push_back(row_id);
            }
            diag_coef = (-1)*accumulate(row_values.begin(), row_values.end(), 0.0);
        }
        else {
            diag_coef = 1;
        }
        rval = mb->tag_get_data(global_id_tag, &(*it), 1, &row_id); MB_CHK_ERR(rval);

        row_values.push_back(diag_coef);
        row_indexes.push_back(row_id);

        A.InsertGlobalValues(row_id, row_values.size(), &row_values[0], &row_indexes[0]);
        B[n++] = pressure;

        row_values.clear();
        row_indexes.clear();
        adjacencies.clear();
    }
    printf("<%d> Done.\n", rank);
    A.FillComplete();
    MPI_Barrier(MPI_COMM_WORLD);

    Epetra_LinearProblem linear_problem (&A, &X, &B);
    AztecOO solver (linear_problem);
    solver.Iterate(100000, 1e-10);
    printf("<%d> Solved it.\n", rank);

    printf("<%d> Setting pressure tag\n", rank);
    Tag pressure_tag;
    rval = mb->tag_get_handle("PRESSURE", 1, MB_TYPE_DOUBLE, pressure_tag, MB_TAG_DENSE | MB_TAG_CREAT);
    MB_CHK_SET_ERR(rval, "tag_get_handle to pressure_tag failed");
    rval = mb->tag_set_data(pressure_tag, my_elems, &X[0]);
    MB_CHK_SET_ERR(rval, "tag_set_data to pressure_tag failed");
    printf("<%d> Done\n", rank);

    EntityHandle volumes_meshset;
    rval = mb->create_meshset(0, volumes_meshset);
    MB_CHK_SET_ERR(rval, "create_meshset failed");
    rval = mb->write_file("solved_mesh.h5m", 0, 0, &volumes_meshset, 1);
    MB_CHK_SET_ERR(rval, "write_file failed");

    // Cleaning up alocated objects.
    delete topo_util;
    delete pcomm;
    delete mb;
    MPI_Finalize();

    return 0;
}
<|endoftext|>


<commit_before>/*****************************************************************************/
/**
 *  @file   Object.cpp
 *  @author Naohisa Sakamoto
 */
/*****************************************************************************/
#include "Object.h"
#include "Array.h"
#include "Value.h"

namespace kvs
{

namespace json
{

void Object::add( const std::string& key, const kvs::Int16 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::UInt16 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Int32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::UInt32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Real32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Real64 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( value ) ) );
}

void Object::add( const std::string& key, const std::string& value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( value ) ) );
}

void Object::add( const std::string& key, const kvs::json::Value& value )
{
    SuperClass::insert( std::make_pair( key, value ) );
}

void Object::add( const std::string& key, const kvs::json::Array& value )
{
    SuperClass::insert( std::make_pair( key, value ) );
}

void Object::add( const std::string& key, const kvs::json::Object& value )
{
    SuperClass::insert( std::make_pair( key, value ) );
}

} // end of namespace json

} // end of namespace kvs
<commit_msg>Update Object.cpp<commit_after>/*****************************************************************************/
/**
 *  @file   Object.cpp
 *  @author Naohisa Sakamoto
 */
/*****************************************************************************/
#include "Object.h"
#include "Array.h"
#include "Value.h"

namespace kvs
{

namespace json
{

void Object::add( const std::string& key, const kvs::Int16 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::UInt16 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Int32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::UInt32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Real32 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( kvs::Real64( value ) ) ) );
}

void Object::add( const std::string& key, const kvs::Real64 value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( value ) ) );
}

void Object::add( const std::string& key, const std::string& value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( value ) ) );
}

void Object::add( const std::string& key, const kvs::json::Value& value )
{
    SuperClass::insert( std::make_pair( key, picojson::value( value ) ) );
}

void Object::add( const std::string& key, const kvs::json::Array& value )
{
    SuperClass::insert( std::make_pair( key, picojson::array( value ) ) );
}

void Object::add( const std::string& key, const kvs::json::Object& value )
{
    SuperClass::insert( std::make_pair( key, picojson::object( value ) ) );
}

} // end of namespace json

} // end of namespace kvs
<|endoftext|>


<commit_before>#include "scope.h"
#include <yuni/core/noncopyable.h>
#include "details/utils/check-for-valid-identifier-name.h"
#include "details/grammar/nany.h"
#include "details/ast/ast.h"
#include "exception.h"

using namespace Yuni;

namespace ny {
namespace ir {
namespace Producer {

namespace {

struct AttributeContext final {
	AttributeContext(AST::Node& node)
		: storage{std::make_unique<Attributes>(node)}
		, attributes(*storage)
	{}
	ShortString32 name;
	ShortString32 value;
	std::unique_ptr<Attributes> storage;
	Attributes& attributes;
};

inline AST::Node* getNodeValue(AST::Node& node) {
	switch (node.children.size()) {
		case 1: return nullptr;
		case 2: return &(node.children[1]);
	}
	throw UnexpectedNode(node, "invalid attribute parameter node");
}

} // namespace

bool Scope::fetchAttributes(AST::Node& node) {
	assert(node.rule == AST::rgAttributes);
	try {
		AttributeContext ctx{node};
		for (auto& child : node.children) {
			// checking for node type
			if (unlikely(child.rule != AST::rgAttributesParameter))
				throw UnexpectedNode(child, "invalid node, not attribute parameter");
			AST::Node* nodevalue = getNodeValue(child);
			AST::Node& nodekey = child.children[0];
			if (unlikely(nodekey.rule != AST::rgEntity))
				throw UnexpectedNode(child, "invalid attribute parameter name type");
			if (nodevalue) {
				if (unlikely(nodevalue->rule != AST::rgEntity))
					return (error(child) << "unsupported expression for attribute ctx.value");
			}
			ctx.name.clear();
			ctx.value.clear();
			if (not AST::appendEntityAsString(ctx.name, nodekey))
				throw UnexpectedNode(child, "invalid entity");
			switch (ctx.name[0]) {
				case 'n': {
					if (ctx.name == "nodiscard") {
						if (unlikely(nodevalue))
							return (error(child) << "no ctx.value expected for attribute '" << ctx.name << '\'');
						break;
					}
				}
				// [[fallthru]]
				case 'p': {
					if (ctx.name == "per") {
						if (unlikely(!nodevalue))
							return (error(child) << "ctx.value expected for attribute '" << ctx.name << '\'');
						AST::appendEntityAsString(ctx.value, *nodevalue);
						if (ctx.value == "thread")
							warning(child) << "ignored attribute 'per: thread'";
						else if (ctx.value == "process")
							warning(child) << "ignored attribute 'per: process'";
						else
							return (error(child) << "invalid 'per' ctx.value");
						break;
					}
				}
				// [[fallthru]]
				case 's': {
					if (ctx.name == "nosuggest") {
						ctx.attributes.flags += Attributes::Flag::doNotSuggest;
						if (unlikely(nodevalue))
							return (error(child) << "the attribute '" << ctx.name << "' does not accept ctx.values");
						break;
					}
				}
				// [[fallthru]]
				case 't': {
					if (ctx.name == "threadproc") {
						ctx.attributes.flags += Attributes::Flag::threadproc;
						if (unlikely(nodevalue))
							return (error(child) << "the attribute '" << ctx.name << "' does not accept ctx.values");
						break;
					}
				}
				// [[fallthru]]
				case '_': {
					if (ctx.name == "__nanyc_builtinalias") {
						if (unlikely(!nodevalue))
							return (error(child) << "ctx.value expected for attribute '" << ctx.name << '\'');
						ctx.attributes.builtinAlias = nodevalue;
						ctx.attributes.flags += Attributes::Flag::builtinAlias;
						break;
					}
					if (ctx.name == "__nanyc_shortcircuit") {
						if (unlikely(!nodevalue))
							return (error(child) << "ctx.value expected for attribute '" << ctx.name << '\'');
						AST::appendEntityAsString(ctx.value, *nodevalue);
						bool isTrue = (ctx.value == "__true");
						if (not isTrue and (ctx.value.empty() or ctx.value != "__false")) {
							error(child.children[1]) << "invalid shortcircuit ctx.value, expected '__false' or '__true', got '"
													 << ctx.value << "'";
							return false;
						}
						if (isTrue)
							ctx.attributes.flags += Attributes::Flag::shortcircuit;
						break;
					}
					if (ctx.name == "__nanyc_synthetic") {
						ctx.attributes.flags += Attributes::Flag::pushSynthetic;
						if (unlikely(nodevalue))
							return (error(child) << "the attribute '" << ctx.name << "' does not accept ctx.values");
						break;
					}
					// [FALLBACK]
					// ignore vendor specific attributes (starting by '__')
					if (ctx.name.size() > 2 and ctx.name[1] == '_') {
						// emit a warning for the unsupported specific nany attributes
						if (unlikely(ctx.name.startsWith("__nanyc_")))
							warning(child) << "unknown nanyc attribute '" << ctx.name << "'";
						break;
					}
				}
				// [[fallthru]]
				default: {
					error(child) << "unknown attribute '" << ctx.name << '\'';
					return false;
				}
			}
		}
		attributes = std::move(ctx.storage);
		return true;
	}
	catch (const UnexpectedNode& e) {
		unexpectedNode(e.node, e.message);
	}
	return false;
}

} // namespace Producer
} // namespace ir
} // namespace ny
<commit_msg>ast2ir: attributes: dispatch table for code simplification<commit_after>#include "scope.h"
#include <yuni/core/noncopyable.h>
#include "details/utils/check-for-valid-identifier-name.h"
#include "details/grammar/nany.h"
#include "details/ast/ast.h"
#include "exception.h"

using namespace Yuni;

namespace ny {
namespace ir {
namespace Producer {

namespace {

struct AttributeContext final {
	AttributeContext(AST::Node& node)
		: storage{std::make_unique<Attributes>(node)}
		, attributes(*storage)
	{}
	ShortString32 name;
	ShortString32 value;
	std::unique_ptr<Attributes> storage;
	Attributes& attributes;
	AST::Node* nodevalue = nullptr;
};

inline AST::Node* getNodeValue(AST::Node& node) {
	switch (node.children.size()) {
		case 1: return nullptr;
		case 2: return &(node.children[1]);
	}
	throw UnexpectedNode(node, "invalid attribute parameter node");
}

void nodiscard(AST::Node&, AttributeContext&) {
	// not implemented
}

void per(AST::Node& node, AttributeContext& ctx) {
	AST::appendEntityAsString(ctx.value, *ctx.nodevalue);
	if (ctx.value == "thread")
		warning(node) << "ignored attribute 'per: thread'";
	else if (ctx.value == "process")
		warning(node) << "ignored attribute 'per: process'";
	else
		throw AttributeError{node, "invalid attribute value (expecting 'thread' or 'process')"};
}

void nosuggest(AST::Node&, AttributeContext& ctx) {
	ctx.attributes.flags += Attributes::Flag::doNotSuggest;
}

void threadproc(AST::Node&, AttributeContext& ctx) {
	ctx.attributes.flags += Attributes::Flag::threadproc;
}

void builtinalias(AST::Node&, AttributeContext& ctx) {
	ctx.attributes.builtinAlias = ctx.nodevalue;
	ctx.attributes.flags += Attributes::Flag::builtinAlias;
}

void shortcircuit(AST::Node& node, AttributeContext& ctx) {
	AST::appendEntityAsString(ctx.value, *ctx.nodevalue);
	bool isTrue = (ctx.value == "__true");
	if (not isTrue and (ctx.value.empty() or ctx.value != "__false"))
		throw UnexpectedNode{node.children[1], "invalid attribute value (expecting '__false' or '__true')"};
	if (isTrue)
		ctx.attributes.flags += Attributes::Flag::shortcircuit;
}

void synthetic(AST::Node&, AttributeContext& ctx) {
	ctx.attributes.flags += Attributes::Flag::pushSynthetic;
}

} // namespace

bool Scope::fetchAttributes(AST::Node& node) {
	assert(node.rule == AST::rgAttributes);
	using Callback = void (*)(AST::Node&, AttributeContext&);
	static const std::unordered_map<AnyString, std::pair<bool, Callback>> dispatch = {
		{"nodiscard",            { false, nodiscard}},
		{"per",                  { true,  per}},
		{"nosuggest",            { false, nosuggest}},
		{"threadproc",           { false, threadproc}},
		//
		{"__nanyc_builtinalias", { true,  builtinalias}},
		{"__nanyc_shortcircuit", { true,  shortcircuit}},
		{"__nanyc_synthetic",    { false, synthetic}}
	};
	try {
		AttributeContext ctx{node};
		for (auto& child : node.children) {
			// checking for node type
			if (unlikely(child.rule != AST::rgAttributesParameter))
				throw UnexpectedNode(child, "invalid node, not attribute parameter");
			ctx.nodevalue = getNodeValue(child);
			AST::Node& nodekey = child.children[0];
			if (unlikely(nodekey.rule != AST::rgEntity))
				throw UnexpectedNode(child, "invalid attribute parameter name type");
			ctx.name.clear();
			ctx.value.clear();
			bool hasName = AST::appendEntityAsString(ctx.name, nodekey);
			if (unlikely(not hasName))
				throw UnexpectedNode(child, "invalid entity");
			auto it = dispatch.find(ctx.name);
			if (unlikely(it == dispatch.end()))
				throw AttributeError{child, "unknown attribute"};
			if (not it->second.first) {
				if (unlikely(ctx.nodevalue))
					throw AttributeError{child, "the attribute does not accept any value"};
			}
			else {
				if (unlikely(ctx.nodevalue == nullptr))
					throw AttributeError{child, "the attribute requires a value"};
			}
			if (ctx.nodevalue) {
				if (unlikely(ctx.nodevalue->rule != AST::rgEntity))
					throw UnexpectedNode{child, "unsupported node type attribute value"};
			}
			it->second.second(child, ctx);
		}
		attributes = std::move(ctx.storage);
		return true;
	}
	catch (const UnexpectedNode& e) {
		unexpectedNode(e.node, e.message);
	}
	catch (const AttributeError& e) {
		error(e.node) << '\'' << e.attrname << "': " << e.message;
	}
	catch (const Error& e) {
		error(e.node) << e.message;
	}
	return false;
}

} // namespace Producer
} // namespace ir
} // namespace ny
<|endoftext|>


<commit_before>// Time:  O(nlogn)
// Space: O(n)

// BST solution. (40ms)
class Solution {
public:
    class BSTreeNode {
    public:
        int val, count;
        BSTreeNode *left, *right;
        BSTreeNode(int val, int count) {
            this->val = val;
            this->count = count;
            this->left = this->right = nullptr;
        }
    };

    vector<int> countSmaller(vector<int>& nums) {
        vector<int> res(nums.size());

        BSTreeNode *root = nullptr;

        // Insert into BST and get left count.
        for (int i = nums.size() - 1; i >= 0; --i) {
            BSTreeNode *node = new BSTreeNode(nums[i], 0);
            root = insertNode(root, node);
            res[i] = query(root, nums[i]);
        }

        return res;
    }

    // Insert node into BST.
    BSTreeNode* insertNode(BSTreeNode* root, BSTreeNode* node) {
        if (root == nullptr) {
            return node;
        }
        BSTreeNode* curr = root;
        while (curr) {
            // Insert left if smaller.
            if (node->val < curr->val) {
                ++curr->count; // Increase the number of left children.
                if (curr->left != nullptr) {
                    curr = curr->left;
                } else {
                    curr->left = node;
                    break;
                }
            } else {  // Insert right if larger or equal.
                if (curr->right != nullptr) {
                    curr = curr->right;
                } else {
                    curr->right = node;
                    break;
                }
            }
        }
        return root;
    }

    // Query the smaller count of the value.
    int query(BSTreeNode* root, int val) {
        if (root == nullptr) {
            return 0;
        }
        int count = 0;
        BSTreeNode* curr = root;
        while (curr) {
            // Insert left.
            if (val < curr->val) {
                curr = curr->left;
            } else if (val > curr->val) {
                count += 1 + curr->count; // Count the number of the smaller nodes.
                curr = curr->right;
            } else {  // Equal.
                return count + curr->count;
            }
        }
        return 0;
    }
};

// Time:  O(nlogn)
// Space: O(n)
// BIT solution. (56ms)
class Solution2 {
public:
    vector<int> countSmaller(vector<int>& nums) {
        // Get the place (position in the ascending order) of each number.
        vector<int> sorted_nums(nums), places(nums.size());
        sort(sorted_nums.begin(), sorted_nums.end());
        for (int i = 0; i < nums.size(); ++i) {
            places[i] =
                lower_bound(sorted_nums.begin(), sorted_nums.end(), nums[i]) -
                sorted_nums.begin();
        }
        // Count the smaller elements after the number.
        vector<int> bit(nums.size() + 1), ans(nums.size());
        for (int i = nums.size() - 1; i >= 0; --i) {
            ans[i] = query(bit, places[i]);
            add(bit, places[i] + 1, 1);
        }
        return ans;
    }

private:
    void add(vector<int>& bit, int i, int val) {
        for (; i < bit.size(); i += lower_bit(i)) {
            bit[i] += val;
        }
    }

    int query(const vector<int>& bit, int i) {
        int sum = 0;
        for (; i > 0; i -= lower_bit(i)) {
            sum += bit[i];
        }
        return sum;
    }

    int lower_bit(int i) {
        return i & -i;
    }
};
<commit_msg>Update count-of-smaller-numbers-after-self.cpp<commit_after>// Time:  O(nlogn)
// Space: O(n)

// BST solution. (40ms)
class Solution {
public:
    class BSTreeNode {
    public:
        int val, count;
        BSTreeNode *left, *right;
        BSTreeNode(int val, int count) {
            this->val = val;
            this->count = count;
            this->left = this->right = nullptr;
        }
    };

    vector<int> countSmaller(vector<int>& nums) {
        vector<int> res(nums.size());

        BSTreeNode *root = nullptr;

        // Insert into BST and get right count.
        for (int i = nums.size() - 1; i >= 0; --i) {
            BSTreeNode *node = new BSTreeNode(nums[i], 0);
            root = insertNode(root, node);
            res[i] = query(root, nums[i]);
        }

        return res;
    }

    // Insert node into BST.
    BSTreeNode* insertNode(BSTreeNode* root, BSTreeNode* node) {
        if (root == nullptr) {
            return node;
        }
        BSTreeNode* curr = root;
        while (curr) {
            // Insert left if smaller.
            if (node->val < curr->val) {
                ++curr->count; // Increase the number of left children.
                if (curr->left != nullptr) {
                    curr = curr->left;
                } else {
                    curr->left = node;
                    break;
                }
            } else {  // Insert right if larger or equal.
                if (curr->right != nullptr) {
                    curr = curr->right;
                } else {
                    curr->right = node;
                    break;
                }
            }
        }
        return root;
    }

    // Query the smaller count of the value.
    int query(BSTreeNode* root, int val) {
        if (root == nullptr) {
            return 0;
        }
        int count = 0;
        BSTreeNode* curr = root;
        while (curr) {
            // Insert left.
            if (val < curr->val) {
                curr = curr->left;
            } else if (val > curr->val) {
                count += 1 + curr->count; // Count the number of the smaller nodes.
                curr = curr->right;
            } else {  // Equal.
                return count + curr->count;
            }
        }
        return 0;
    }
};

// Time:  O(nlogn)
// Space: O(n)
// BIT solution. (56ms)
class Solution2 {
public:
    vector<int> countSmaller(vector<int>& nums) {
        // Get the place (position in the ascending order) of each number.
        vector<int> sorted_nums(nums), places(nums.size());
        sort(sorted_nums.begin(), sorted_nums.end());
        for (int i = 0; i < nums.size(); ++i) {
            places[i] =
                lower_bound(sorted_nums.begin(), sorted_nums.end(), nums[i]) -
                sorted_nums.begin();
        }
        // Count the smaller elements after the number.
        vector<int> bit(nums.size() + 1), ans(nums.size());
        for (int i = nums.size() - 1; i >= 0; --i) {
            ans[i] = query(bit, places[i]);
            add(bit, places[i] + 1, 1);
        }
        return ans;
    }

private:
    void add(vector<int>& bit, int i, int val) {
        for (; i < bit.size(); i += lower_bit(i)) {
            bit[i] += val;
        }
    }

    int query(const vector<int>& bit, int i) {
        int sum = 0;
        for (; i > 0; i -= lower_bit(i)) {
            sum += bit[i];
        }
        return sum;
    }

    int lower_bit(int i) {
        return i & -i;
    }
};
<|endoftext|>


<commit_before>#pragma once

#include <tuple>

namespace kgr {
namespace detail {

template <typename T>
struct function_traits
	: function_traits<decltype(&T::operator())>
{};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const> {
	using object_type = Type;
	using return_type = R;
	using argument_types = std::tuple<Args...>;
	template<int n> using argument_type = typename std::tuple_element<n, argument_types>::type;
};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...)> {
	using object_type = Type;
	using return_type = R;
	using argument_types = std::tuple<Args...>;
	template<int n> using argument_type = typename std::tuple_element<n, argument_types>::type;
};

template <typename R, typename... Args>
struct function_traits<R(*)(Args...)> {
	using return_type = R;
	using argument_types = std::tuple<Args...>;
	template<int n> using argument_type = typename std::tuple_element<n, argument_types>::type;
};

template <typename F>
using function_arguments_t = typename function_traits<F>::argument_types;

template <typename F>
using function_result_t = typename function_traits<F>::return_type;

template <typename F>
using object_type_t = typename function_traits<F>::object_type;

template <int n, typename F>
using function_argument_t = typename function_traits<F>::template argument_type<n>;

} // namespace detail
} // namespace kgr
<commit_msg>added support for abominable function types<commit_after>#pragma once

#include <tuple>

namespace kgr {
namespace detail {

template <typename T>
struct function_traits
	: function_traits<decltype(&T::operator())>
{};

template <typename Type, typename R, typename... Args>
struct base_function_traits {
	using object_type = Type;
	using return_type = R;
	using argument_types = std::tuple<Args...>;
	template<int n> using argument_type = typename std::tuple_element<n, argument_types>::type;
};

template <typename R, typename... Args>
struct base_non_member_function_traits {
	using return_type = R;
	using argument_types = std::tuple<Args...>;
	template<int n> using argument_type = typename std::tuple_element<n, argument_types>::type;
};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...)> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const volatile> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) volatile> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const volatile &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) volatile &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) const volatile &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args...) volatile &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...)> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const volatile> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) volatile> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const volatile &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) volatile &> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) const volatile &&> : base_function_traits<Type, R, Args...> {};

template <typename Type, typename R, typename... Args>
struct function_traits<R(Type::*)(Args..., ...) volatile &&> : base_function_traits<Type, R, Args...> {};

template <typename R, typename... Args>
struct function_traits<R(*)(Args...)> : base_non_member_function_traits<R, Args...> {};

template <typename R, typename... Args>
struct function_traits<R(*)(Args..., ...)> : base_non_member_function_traits<R, Args...> {};

template <typename F>
using function_arguments_t = typename function_traits<F>::argument_types;

template <typename F>
using function_result_t = typename function_traits<F>::return_type;

template <typename F>
using object_type_t = typename function_traits<F>::object_type;

template <int n, typename F>
using function_argument_t = typename function_traits<F>::template argument_type<n>;

} // namespace detail
} // namespace kgr
<|endoftext|>


<commit_before>#include <ros/ros.h> //For ROS itself
//#include <image_transport/image_transport.h> //For the image transport class. Still needs the vc bridge though.
#include <opencv2/highgui/highgui.hpp> //FIXME Not exactly sure why this one is here; maybe it's for displaying the images that come in?
#include <cv_bridge/cv_bridge.h> //The openCV bridge that image_transport needs
#include <ros/console.h> //This is for ROS_ERROR and stuff.
#include <boost/filesystem.hpp> //For creating directtories if needed
#include <obe_toolset/ImageAndPose.h> //Needed for the custom Image and Pose message type
#include <string>
#include <vector>

//#include "std_msgs/String.h"
#define ONEATATIME //if this is defined, it will do one file at a time and pause in between files.

int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_publisher_node");
	ros::NodeHandle n;
	int numOfPipes = 1;
	int currentPipeMinusOne = 0;
	if(!(n.hasParam("/DispatcherNode/numPipes"))) //FIXME this might need to be /FakeDispatcher/numPipes, but it's left like this so the transition to the real dispatcher won't end up not having a numPipes argument
		ROS_INFO("the parameter 'numPipes' for the dispatcher node hasn't been specified; assuming 1 to ensure that no images are lost. This may cause severe back-up issues in a long mission.");
	n.param<int>("numPipes", numOfPipes, 1); //gets the numPipes param (so that we know where to publish), defaulting to 1 if it can't be read.
	ROS_INFO("%d", numOfPipes);

	std::vector<ros::Publisher> impose_pub_vector(numOfPipes); //vector of publishers
	for(int i = 1; i <= numOfPipes; ++i)
	{
		std::string topic("obe/imagePipe");
		topic += std::to_string(i);
		impose_pub_vector[i-1] = n.advertise<obe_toolset::ImageAndPose>(topic.c_str(), 512); //publishes to the obe/imagePipe<i> and buffers up to 512 images per pipe in case it can't send them all that quickly.
	}

//	image_transport::ImageTransport it(n);
//	ros::Publisher pub = n.advertise<std_msgs::String>("obe/test", 100);
//	image_transport::Publisher impose_pub = it.advertise("obe/image", 255); //Buffers up to 255 pictures in case connection is lost
//	^^That used to be the old publisher.
//	cv::Mat image = cv::imread("/home/kennon/images/BrentRambo.jpg", CV_LOAD_IMAGE_COLOR);
//	cv::waitKey(30); //No idea what this line does...
//	cv::imshow("view", image); //This can be used to see the image as it's published

	//This block makes sure that the needed directories are set up (I'm pretty sure they should be, since this node might end up running from one of them).
	namespace fs = boost::filesystem;

	fs::path new_path("../images/new/"); //Will be used to get images.
	fs::path processed_path("../images/processed/");
    fs::path roi_path("../images/rois/");
	fs::path error_path("../images/unsuccessful/");

	fs::path im_fetch_path(new_path);

	fs::create_directories(im_fetch_path);
	fs::create_directories(processed_path);
    fs::create_directories(roi_path);
	fs::create_directories(error_path);
	//NOTE:: IF THERE IS AN ISSUE WITH PERMISSIONS FOR SOME REASON, IT MIGHT BE THAT THE LINES OF CODE ABOVE ARE REMOVING EXECUTE PERMISSIONS. JUST SOMETHING TO CONSIDER

	if (!(fs::exists(im_fetch_path)))
	{
		//If it doesn't exist, something went horribly wrong =/
		ROS_ERROR("Oh no. '%s' isn't a directory =(", im_fetch_path.string().c_str());
		return(-1);
	}
	else if (fs::is_regular_file(im_fetch_path))
	{
		ROS_ERROR("Oh no. '%s' is a regular file =(", im_fetch_path.string().c_str());
		return(-1);
	}

	#ifdef ONEATATIME
	ros::Rate loop_rate(1); // 1 per second is fairly realistic
	#else
	ros::Rate loop_rate(25); //rate is the number of outer loop iterations per second
	//Also, we need the loop rate to be relatively quick becuase we don't want delay in dispatching images. Delay in dispatching images could lead to wasted execution time and potential incorrect location stamps.
	#endif //ONEATATIME

	while(n.ok())
	{
		//We need to dispatch each of the images in the new directory
//		ROS_INFO("Looking at path %s", im_fetch_path.string().c_str());
		fs::directory_iterator end_itr;
		for(fs::directory_iterator cur_path_itr(im_fetch_path); cur_path_itr != end_itr; cur_path_itr++)
		{
//			ROS_INFO("I'm looking at this file: %s", cur_path_itr->path().string().c_str());
			if(fs::is_directory(cur_path_itr->path()))
			{
				ROS_INFO_ONCE("There's a directory in the ~/images/new path; this is currently unhandled.");
				fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());

				//FIXME Don't forget to move this file to the folder for files with errors.

			}
			else
			{
				cv::Mat newImage = cv::imread(cur_path_itr->path().string(), CV_LOAD_IMAGE_COLOR);

				if(newImage.data == NULL)//The image was read improperly if this fails...
				{
					ROS_ERROR("I was unable to read the file image file.");
					fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
				}
				else
				{
					obe_toolset::ImageAndPose impose_msg; //This is the custom message type that has a sensor_msgs::Image msg and a std_msgs::Pose message.
					impose_msg.image = *(cv_bridge::CvImage(std_msgs::Header(), "bgr8", newImage).toImageMsg()); //The meat of this line is from the image_transport tutorial; I just de-reference their piece to get a sensor_msgs::Image.  Note: There's probably a better way to do this, but it will work for now.

					//This is the point where the fakeing things comes in.
					impose_msg.x = 0; //fakes an x UTM value
					impose_msg.y = 0; //etc...
					impose_msg.z = 60.96; //This is actually a decent guess (~200 ft)
					impose_msg.roll = 0.0;
					impose_msg.pitch = 0.0;
					impose_msg.yaw = 0.0; //this is the only one that's used as of 4.26.17
					//End the faking it stuff.

					//publish to the current pipe that's due for another message. NOTE: In the future, this could have a system that keeps track of busy nodes so that no particular node gets bogged down. I'm kind of assuming that we have enough nodes and a fast enough ROI algorithm and randomness is on our side so that this doesn't get out of hand.
					impose_pub_vector[currentPipeMinusOne].publish(impose_msg); //send the impose message.

					//set up the current pipe for the next time we publish something.
					currentPipeMinusOne++;
					currentPipeMinusOne %= numOfPipes; //we want to wrap around

					//Now that we've published it, we can move the file to the processed folder
					fs::rename(cur_path_itr->path(), processed_path / cur_path_itr->path().filename());
					#ifdef ONEATATIME
					break;
					#endif
				}
			}
		} //Ends the for loop
		ros::spinOnce();
		loop_rate.sleep();
	}
	//It shouldn't get here until you hit ctrl-C, but we still need to specify a return value:
	return 0;
}
<commit_msg>Fixed the bug where the FakeDispatcher would misread the ros param if launched from a launchfile<commit_after>#include <ros/ros.h> //For ROS itself
//#include <image_transport/image_transport.h> //For the image transport class. Still needs the vc bridge though.
#include <opencv2/highgui/highgui.hpp> //FIXME Not exactly sure why this one is here; maybe it's for displaying the images that come in?
#include <cv_bridge/cv_bridge.h> //The openCV bridge that image_transport needs
#include <ros/console.h> //This is for ROS_ERROR and stuff.
#include <boost/filesystem.hpp> //For creating directtories if needed
#include <obe_toolset/ImageAndPose.h> //Needed for the custom Image and Pose message type
#include <string>
#include <vector>

//#include "std_msgs/String.h"
#define ONEATATIME //if this is defined, it will do one file at a time and pause in between files.

static const std::string NumPipesParamPath("/DispatcherNode/numPipes");

int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_publisher_node");
	ros::NodeHandle n;
	int numOfPipes = 1;
	int currentPipeMinusOne = 0;
	if(!(n.hasParam(NumPipesParamPath))) //FIXME this might need to be /FakeDispatcher/numPipes, but it's left like this so the transition to the real dispatcher won't end up not having a numPipes argument
		ROS_INFO("the parameter 'numPipes' for the dispatcher node hasn't been specified; assuming 1 to ensure that no images are lost. This may cause severe back-up issues in a long mission.");
	n.param<int>(NumPipesParamPath, numOfPipes, 1); //gets the numPipes param (so that we know where to publish), defaulting to 1 if it can't be read.
	ROS_INFO("%d pipes passed in.", numOfPipes);

	std::vector<ros::Publisher> impose_pub_vector(numOfPipes); //vector of publishers
	for(int i = 1; i <= numOfPipes; ++i)
	{
		std::string topic("obe/imagePipe");
		topic += std::to_string(i);
		impose_pub_vector[i-1] = n.advertise<obe_toolset::ImageAndPose>(topic.c_str(), 512); //publishes to the obe/imagePipe<i> and buffers up to 512 images per pipe in case it can't send them all that quickly.
	}

//	image_transport::ImageTransport it(n);
//	ros::Publisher pub = n.advertise<std_msgs::String>("obe/test", 100);
//	image_transport::Publisher impose_pub = it.advertise("obe/image", 255); //Buffers up to 255 pictures in case connection is lost
//	^^That used to be the old publisher.
//	cv::Mat image = cv::imread("/home/kennon/images/BrentRambo.jpg", CV_LOAD_IMAGE_COLOR);
//	cv::waitKey(30); //No idea what this line does...
//	cv::imshow("view", image); //This can be used to see the image as it's published

	//This block makes sure that the needed directories are set up (I'm pretty sure they should be, since this node might end up running from one of them).
	namespace fs = boost::filesystem;

	fs::path new_path("../images/new/"); //Will be used to get images.
	fs::path processed_path("../images/processed/");
    fs::path roi_path("../images/rois/");
	fs::path error_path("../images/unsuccessful/");

	fs::path im_fetch_path(new_path);

	fs::create_directories(im_fetch_path);
	fs::create_directories(processed_path);
    fs::create_directories(roi_path);
	fs::create_directories(error_path);
	//NOTE:: IF THERE IS AN ISSUE WITH PERMISSIONS FOR SOME REASON, IT MIGHT BE THAT THE LINES OF CODE ABOVE ARE REMOVING EXECUTE PERMISSIONS. JUST SOMETHING TO CONSIDER

	if (!(fs::exists(im_fetch_path)))
	{
		//If it doesn't exist, something went horribly wrong =/
		ROS_ERROR("Oh no. '%s' isn't a directory =(", im_fetch_path.string().c_str());
		return(-1);
	}
	else if (fs::is_regular_file(im_fetch_path))
	{
		ROS_ERROR("Oh no. '%s' is a regular file =(", im_fetch_path.string().c_str());
		return(-1);
	}

	#ifdef ONEATATIME
	ros::Rate loop_rate(1); // 1 per second is fairly realistic
	#else
	ros::Rate loop_rate(25); //rate is the number of outer loop iterations per second
	//Also, we need the loop rate to be relatively quick becuase we don't want delay in dispatching images. Delay in dispatching images could lead to wasted execution time and potential incorrect location stamps.
	#endif //ONEATATIME

	while(n.ok())
	{
		//We need to dispatch each of the images in the new directory
//		ROS_INFO("Looking at path %s", im_fetch_path.string().c_str());
		fs::directory_iterator end_itr;
		for(fs::directory_iterator cur_path_itr(im_fetch_path); cur_path_itr != end_itr; cur_path_itr++)
		{
//			ROS_INFO("I'm looking at this file: %s", cur_path_itr->path().string().c_str());
			if(fs::is_directory(cur_path_itr->path()))
			{
				ROS_INFO_ONCE("There's a directory in the ~/images/new path; this is currently unhandled.");
				fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());

				//FIXME Don't forget to move this file to the folder for files with errors.

			}
			else
			{
				cv::Mat newImage = cv::imread(cur_path_itr->path().string(), CV_LOAD_IMAGE_COLOR);

				if(newImage.data == NULL)//The image was read improperly if this fails...
				{
					ROS_ERROR("I was unable to read the file image file.");
					fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
				}
				else
				{
					obe_toolset::ImageAndPose impose_msg; //This is the custom message type that has a sensor_msgs::Image msg and a std_msgs::Pose message.
					impose_msg.image = *(cv_bridge::CvImage(std_msgs::Header(), "bgr8", newImage).toImageMsg()); //The meat of this line is from the image_transport tutorial; I just de-reference their piece to get a sensor_msgs::Image.  Note: There's probably a better way to do this, but it will work for now.

					//This is the point where the fakeing things comes in.
					impose_msg.x = 0; //fakes an x UTM value
					impose_msg.y = 0; //etc...
					impose_msg.z = 60.96; //This is actually a decent guess (~200 ft)
					impose_msg.roll = 0.0;
					impose_msg.pitch = 0.0;
					impose_msg.yaw = 0.0; //this is the only one that's used as of 4.26.17
					//End the faking it stuff.

					//publish to the current pipe that's due for another message. NOTE: In the future, this could have a system that keeps track of busy nodes so that no particular node gets bogged down. I'm kind of assuming that we have enough nodes and a fast enough ROI algorithm and randomness is on our side so that this doesn't get out of hand.
					impose_pub_vector[currentPipeMinusOne].publish(impose_msg); //send the impose message.
					ROS_INFO("Sending to pipe %d of %d", currentPipeMinusOne + 1, numOfPipes);

					//set up the current pipe for the next time we publish something.
					currentPipeMinusOne++;
					currentPipeMinusOne %= numOfPipes; //we want to wrap around

					//Now that we've published it, we can move the file to the processed folder
					fs::rename(cur_path_itr->path(), processed_path / cur_path_itr->path().filename());
					#ifdef ONEATATIME
					break;
					#endif
				}
			}
		} //Ends the for loop
		ros::spinOnce();
		loop_rate.sleep();
	}
	//It shouldn't get here until you hit ctrl-C, but we still need to specify a return value:
	return 0;
}
<|endoftext|>