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

#include <assert.h>

#include <algorithm>

#include <stout/none.hpp>
#include <stout/some.hpp>

template <typename T>
class Option
{
public:
  static Option<T> none()
  {
    return Option<T>();
  }

  static Option<T> some(const T& t)
  {
    return Option<T>(t);
  }

  Option() : state(NONE) {}

  Option(const T& _t) : state(SOME), t(_t) {}

  template <typename U>
  Option(const U& u) : state(SOME), t(u) {}

  Option(const None& none) : state(NONE) {}

  template <typename U>
  Option(const _Some<U>& some) : Option(some.t) {}

  Option(const Option<T>& that) : state(that.state)
  {
    if (that.isSome()) {
      new (&t) T(that.t);
    }
  }

  ~Option()
  {
    if (isSome()) {
      t.~T();
    }
  }

  Option<T>& operator = (const Option<T>& that)
  {
    if (this != &that) {
      if (isSome()) {
        t.~T();
      }
      state = that.state;
      if (that.isSome()) {
        new (&t) T(that.t);
      }
    }

    return *this;
  }

  bool operator == (const Option<T>& that) const
  {
    return (isNone() && that.isNone()) ||
      (isSome() && that.isSome() && t == that.t);
  }

  bool operator != (const Option<T>& that) const
  {
    return !operator == (that);
  }

  bool operator == (const T& that) const
  {
    return isSome() && t == that;
  }

  bool operator != (const T& that) const
  {
    return !operator == (that);
  }

  bool isSome() const { return state == SOME; }
  bool isNone() const { return state == NONE; }

  const T& get() const { assert(isSome()); return t; }

  // This must return a copy to avoid returning a reference to a temporary.
  T get(const T& _t) const { return isNone() ? _t : t; }

private:
  enum State {
    SOME,
    NONE,
  };

  State state;

  union {
    // We remove the const qualifier (if there is one) from T so that
    // we can initialize 't' from outside of the initializer list
    // using placement new. This is necessary because sometimes we
    // specialize 'Option' as such: 'Option<const T>'.
    typename std::remove_const<T>::type t;
  };
};

template <typename T>
Option<T> min(const Option<T>& left, const Option<T>& right)
{
  if (left.isSome() && right.isSome()) {
    return std::min(left.get(), right.get());
  } else if (left.isSome()) {
    return left.get();
  } else if (right.isSome()) {
    return right.get();
  } else {
    return Option<T>::none();
  }
}

template <typename T>
Option<T> min(const Option<T>& left, const T& right)
{
  return min(left, Option<T>(right));
}

template <typename T>
Option<T> min(const T& left, const Option<T>& right)
{
  return min(Option<T>(left), right);
}

template <typename T>
Option<T> max(const Option<T>& left, const Option<T>& right)
{
  if (left.isSome() && right.isSome()) {
    return std::max(left.get(), right.get());
  } else if (left.isSome()) {
    return left.get();
  } else if (right.isSome()) {
    return right.get();
  } else {
    return Option<T>::none();
  }
}

template <typename T>
Option<T> max(const Option<T>& left, const T& right)
{
  return max(left, Option<T>(right));
}

template <typename T>
Option<T> max(const T& left, const Option<T>& right)
{
  return max(Option<T>(left), right);
}

#endif // __STOUT_OPTION_HPP__
<commit_msg>Use special constructor for Option<T> from Some<T>.<commit_after>/**
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef __STOUT_OPTION_HPP__
#define __STOUT_OPTION_HPP__

#include <assert.h>

#include <algorithm>

#include <stout/none.hpp>
#include <stout/some.hpp>

template <typename T>
class Option
{
public:
  static Option<T> none()
  {
    return Option<T>();
  }

  static Option<T> some(const T& t)
  {
    return Option<T>(t);
  }

  Option() : state(NONE) {}

  Option(const T& _t) : state(SOME), t(_t) {}

  template <typename U>
  Option(const U& u) : state(SOME), t(u) {}

  Option(const None& none) : state(NONE) {}

  template <typename U>
  Option(const _Some<U>& some) : state(SOME), t(some.t) {}

  Option(const Option<T>& that) : state(that.state)
  {
    if (that.isSome()) {
      new (&t) T(that.t);
    }
  }

  ~Option()
  {
    if (isSome()) {
      t.~T();
    }
  }

  Option<T>& operator = (const Option<T>& that)
  {
    if (this != &that) {
      if (isSome()) {
        t.~T();
      }
      state = that.state;
      if (that.isSome()) {
        new (&t) T(that.t);
      }
    }

    return *this;
  }

  bool operator == (const Option<T>& that) const
  {
    return (isNone() && that.isNone()) ||
      (isSome() && that.isSome() && t == that.t);
  }

  bool operator != (const Option<T>& that) const
  {
    return !operator == (that);
  }

  bool operator == (const T& that) const
  {
    return isSome() && t == that;
  }

  bool operator != (const T& that) const
  {
    return !operator == (that);
  }

  bool isSome() const { return state == SOME; }
  bool isNone() const { return state == NONE; }

  const T& get() const { assert(isSome()); return t; }

  // This must return a copy to avoid returning a reference to a temporary.
  T get(const T& _t) const { return isNone() ? _t : t; }

private:
  enum State {
    SOME,
    NONE,
  };

  State state;

  union {
    // We remove the const qualifier (if there is one) from T so that
    // we can initialize 't' from outside of the initializer list
    // using placement new. This is necessary because sometimes we
    // specialize 'Option' as such: 'Option<const T>'.
    typename std::remove_const<T>::type t;
  };
};

template <typename T>
Option<T> min(const Option<T>& left, const Option<T>& right)
{
  if (left.isSome() && right.isSome()) {
    return std::min(left.get(), right.get());
  } else if (left.isSome()) {
    return left.get();
  } else if (right.isSome()) {
    return right.get();
  } else {
    return Option<T>::none();
  }
}

template <typename T>
Option<T> min(const Option<T>& left, const T& right)
{
  return min(left, Option<T>(right));
}

template <typename T>
Option<T> min(const T& left, const Option<T>& right)
{
  return min(Option<T>(left), right);
}

template <typename T>
Option<T> max(const Option<T>& left, const Option<T>& right)
{
  if (left.isSome() && right.isSome()) {
    return std::max(left.get(), right.get());
  } else if (left.isSome()) {
    return left.get();
  } else if (right.isSome()) {
    return right.get();
  } else {
    return Option<T>::none();
  }
}

template <typename T>
Option<T> max(const Option<T>& left, const T& right)
{
  return max(left, Option<T>(right));
}

template <typename T>
Option<T> max(const T& left, const Option<T>& right)
{
  return max(Option<T>(left), right);
}

#endif // __STOUT_OPTION_HPP__
<|endoftext|>


<commit_before>/**
 * @file EM.cpp
 * @brief mex interface for EM
 * @author Vladimir Eremeev
 * @date 2012
 */
#include "mexopencv.hpp"
#include "opencv2/ml/ml.hpp"
using namespace std;
using namespace cv;

// Persistent objects

/// Last object id to allocate
int last_id = 0;
/// Object container
map<int,EM> obj_;

/// CovMatType map for option processing
const ConstMap<string, int> CovMatType = ConstMap<string, int>
    ("Spherical", EM::COV_MAT_SPHERICAL)
    ("Diagonal",  EM::COV_MAT_DIAGONAL)
    ("Generic",   EM::COV_MAT_GENERIC);

/// CovMatTypeInv map for option processing
const ConstMap<int, string> CovMatTypeInv = ConstMap<int, string>
    (EM::COV_MAT_SPHERICAL, "Spherical")
    (EM::COV_MAT_DIAGONAL,  "Diagonal")
    (EM::COV_MAT_GENERIC,   "Generic");

// convenience macro to return results
#define assign_lhs(ll, label, prob)  do { \
    plhs[0] = MxArray(ll); \
if (nlhs > 1) \
    plhs[1] = MxArray(label); \
if (nlhs > 2) \
    plhs[2] = MxArray(prob); \
} while(0)

/**
 * Main entry called from Matlab
 * @param nlhs number of left-hand-side arguments
 * @param plhs pointers to mxArrays in the left-hand-side
 * @param nrhs number of right-hand-side arguments
 * @param prhs pointers to mxArrays in the right-hand-side
 */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    // Check the number of arguments
    if (nrhs < 2 || nlhs > 3)
        mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");

    // Argument vector
    vector<MxArray> rhs(prhs,prhs+nrhs);

    // Determine argument format between constructor or (id,method,...)
    int id = 0;
    string method;
    if (nrhs > 1 && rhs[0].isNumeric() && rhs[1].isChar()) {
        id = rhs[0].toInt();
        method = rhs[1].toString();
    } else {
        mexErrMsgIdAndTxt("mexopencv:error","Invalid arguments");
    }

    // Big operation switch
    if (method == "new") {
        if (nrhs > 3  && (nrhs % 2)==0) {

            int nclusters = EM::DEFAULT_NCLUSTERS;
            int covMatType = EM::COV_MAT_DIAGONAL;
            int maxIter = EM::DEFAULT_MAX_ITERS;
            double eps = FLT_EPSILON;

            for(int i = 2; i < nrhs; i += 2) {
                string key(rhs[i].toString());
                if (key == "Nclusters") {
                    nclusters = rhs[i + 1].toInt();
                } else if (key == "CovMatType") {
                    covMatType = CovMatType[rhs[i + 1].toString()];
                } else if (key == "MaxIters") {
                    maxIter = rhs[i + 1].toInt();
                } else if (key == "Epsilon") {
                    eps = rhs[i + 1].toDouble();
                }
            }

            obj_[++last_id] = EM(nclusters, covMatType, TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, maxIter, eps));
        } else if (nrhs == 2)
            obj_[++last_id] = EM();
        else
            mexErrMsgIdAndTxt("mexopencv:error","Invalid arguments");

        plhs[0] = MxArray(last_id);
        return;
    }

    EM& obj = obj_[id];
    if (method == "delete") {
        if (nrhs != 2 || nlhs != 0)
            mexErrMsgIdAndTxt("mexopencv:error","Output not assigned");
        obj_.erase(id);
    } else if (method == "train") {
        if (nrhs != 3)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        Mat samples = rhs[2].toMat();
        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.train(samples, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "trainE") {
        if (nrhs < 4 || (nrhs % 2) != 0)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        Mat samples = rhs[2].toMat();
        Mat means0  = rhs[3].toMat();
        // transpose matrix for convenience
        if (means0.cols == obj.get<int>("nclusters") && means0.rows == samples.cols)
            means0 = means0.t();
        Mat covs0;
        Mat weights0;
        for(int i = 4; i < nrhs; i += 2) {
            string key = rhs[i].toString();
            if (key == "Covs0") {
                covs0 = rhs[i + 1].toMat();
            } else if (key == "Weights0") {
                weights0 = rhs[i + 1].toMat();
            }
        }

        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.trainE(samples, means0, covs0, weights0, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "trainM") {
        if (nrhs < 4)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");

        Mat samples = rhs[2].toMat();
        Mat probs0  = rhs[3].toMat();
        // transpose matrix for convenience
        if (probs0.rows == obj.get<int>("nclusters") && probs0.cols == samples.rows)
            probs0 = probs0.t();
        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.trainM(samples, probs0, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "nclusters") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toInt());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<int>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "covMatType") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, CovMatType[rhs[2].toString()]);
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(CovMatTypeInv[obj.get<int>(method)]);
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "maxIters") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toInt());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<int>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "epsilon") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toDouble());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<double>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "weights" || method == "means") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<Mat>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "covs") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<vector<Mat> >(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "isTrained") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.isTrained());
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "save") {
        if (nrhs == 3 && nlhs == 0) {
            string filename = rhs[2].toString();
            FileStorage fs(filename, FileStorage::WRITE);
            if (fs.isOpened()) {
                obj.write(fs);
            } else {
                ostringstream emsg;
                emsg << "Could not open file " << filename << " for writing";
                mexErrMsgIdAndTxt("mexopencv:error",emsg.str().c_str());
            }
        } else {
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        }
    } else if (method == "load") {
        if (nrhs == 3 && nlhs == 0) {
            string filename = rhs[2].toString();
            const FileStorage fs(filename, FileStorage::READ);
            if (fs.isOpened()) {
                const FileNode& fn = fs["StatModel.EM"];
                obj.read(fn);
            } else {
                ostringstream emsg;
                emsg << "Could not open file " << filename << " for reading";
                mexErrMsgIdAndTxt("mexopencv:error",emsg.str().c_str());
            }
        } else {
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        }
    } else if (method == "predict") {
        if (nrhs != 3)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of input arguments");
        Mat sample = rhs[2].toMat();
        Mat probs;
        Vec2d result = obj.predict(sample, probs);
        assign_lhs(result[0], result[1], probs);
    }
}
<commit_msg>Fixed cov0 type in EM<commit_after>/**
 * @file EM.cpp
 * @brief mex interface for EM
 * @author Vladimir Eremeev
 * @date 2012
 */
#include "mexopencv.hpp"
#include "opencv2/ml/ml.hpp"
using namespace std;
using namespace cv;

// Persistent objects

/// Last object id to allocate
int last_id = 0;
/// Object container
map<int,EM> obj_;

/// CovMatType map for option processing
const ConstMap<string, int> CovMatType = ConstMap<string, int>
    ("Spherical", EM::COV_MAT_SPHERICAL)
    ("Diagonal",  EM::COV_MAT_DIAGONAL)
    ("Generic",   EM::COV_MAT_GENERIC);

/// CovMatTypeInv map for option processing
const ConstMap<int, string> CovMatTypeInv = ConstMap<int, string>
    (EM::COV_MAT_SPHERICAL, "Spherical")
    (EM::COV_MAT_DIAGONAL,  "Diagonal")
    (EM::COV_MAT_GENERIC,   "Generic");

// convenience macro to return results
#define assign_lhs(ll, label, prob)  do { \
    plhs[0] = MxArray(ll); \
if (nlhs > 1) \
    plhs[1] = MxArray(label); \
if (nlhs > 2) \
    plhs[2] = MxArray(prob); \
} while(0)

/**
 * Main entry called from Matlab
 * @param nlhs number of left-hand-side arguments
 * @param plhs pointers to mxArrays in the left-hand-side
 * @param nrhs number of right-hand-side arguments
 * @param prhs pointers to mxArrays in the right-hand-side
 */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    // Check the number of arguments
    if (nrhs < 2 || nlhs > 3)
        mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");

    // Argument vector
    vector<MxArray> rhs(prhs,prhs+nrhs);

    // Determine argument format between constructor or (id,method,...)
    int id = 0;
    string method;
    if (nrhs > 1 && rhs[0].isNumeric() && rhs[1].isChar()) {
        id = rhs[0].toInt();
        method = rhs[1].toString();
    } else {
        mexErrMsgIdAndTxt("mexopencv:error","Invalid arguments");
    }

    // Big operation switch
    if (method == "new") {
        if (nrhs > 3  && (nrhs % 2)==0) {

            int nclusters = EM::DEFAULT_NCLUSTERS;
            int covMatType = EM::COV_MAT_DIAGONAL;
            int maxIter = EM::DEFAULT_MAX_ITERS;
            double eps = FLT_EPSILON;

            for(int i = 2; i < nrhs; i += 2) {
                string key(rhs[i].toString());
                if (key == "Nclusters") {
                    nclusters = rhs[i + 1].toInt();
                } else if (key == "CovMatType") {
                    covMatType = CovMatType[rhs[i + 1].toString()];
                } else if (key == "MaxIters") {
                    maxIter = rhs[i + 1].toInt();
                } else if (key == "Epsilon") {
                    eps = rhs[i + 1].toDouble();
                }
            }

            obj_[++last_id] = EM(nclusters, covMatType, TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, maxIter, eps));
        } else if (nrhs == 2)
            obj_[++last_id] = EM();
        else
            mexErrMsgIdAndTxt("mexopencv:error","Invalid arguments");

        plhs[0] = MxArray(last_id);
        return;
    }

    EM& obj = obj_[id];
    if (method == "delete") {
        if (nrhs != 2 || nlhs != 0)
            mexErrMsgIdAndTxt("mexopencv:error","Output not assigned");
        obj_.erase(id);
    } else if (method == "train") {
        if (nrhs != 3)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        Mat samples = rhs[2].toMat();
        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.train(samples, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "trainE") {
        if (nrhs < 4 || (nrhs % 2) != 0)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        Mat samples = rhs[2].toMat();
        Mat means0  = rhs[3].toMat();
        // transpose matrix for convenience
        if (means0.cols == obj.get<int>("nclusters") && means0.rows == samples.cols)
            means0 = means0.t();
        vector<Mat> covs0;
        Mat weights0;
        for(int i = 4; i < nrhs; i += 2) {
            string key = rhs[i].toString();
            if (key == "Covs0") {
                covs0 = rhs[i + 1].toVector<Mat>();
            } else if (key == "Weights0") {
                weights0 = rhs[i + 1].toMat();
            }
        }

        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.trainE(samples, means0, covs0, weights0, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "trainM") {
        if (nrhs < 4)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");

        Mat samples = rhs[2].toMat();
        Mat probs0  = rhs[3].toMat();
        // transpose matrix for convenience
        if (probs0.rows == obj.get<int>("nclusters") && probs0.cols == samples.rows)
            probs0 = probs0.t();
        Mat log_likelihoods;
        Mat labels;
        Mat probs;
        obj.trainM(samples, probs0, log_likelihoods, labels, probs);
        assign_lhs(log_likelihoods, labels, probs);
    } else if (method == "nclusters") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toInt());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<int>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "covMatType") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, CovMatType[rhs[2].toString()]);
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(CovMatTypeInv[obj.get<int>(method)]);
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "maxIters") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toInt());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<int>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "epsilon") {
        if (nrhs == 3 && nlhs == 0)
            obj.set(method, rhs[2].toDouble());
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<double>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "weights" || method == "means") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<Mat>(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "covs") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.get<vector<Mat> >(method));
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "isTrained") {
        if (nrhs == 3 && nlhs == 0)
            mexErrMsgIdAndTxt("mexopencv:error","Attempt to set read-only property");
        else if (nrhs == 2 && nlhs == 1)
            plhs[0] = MxArray(obj.isTrained());
        else
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
    } else if (method == "save") {
        if (nrhs == 3 && nlhs == 0) {
            string filename = rhs[2].toString();
            FileStorage fs(filename, FileStorage::WRITE);
            if (fs.isOpened()) {
                obj.write(fs);
            } else {
                ostringstream emsg;
                emsg << "Could not open file " << filename << " for writing";
                mexErrMsgIdAndTxt("mexopencv:error",emsg.str().c_str());
            }
        } else {
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        }
    } else if (method == "load") {
        if (nrhs == 3 && nlhs == 0) {
            string filename = rhs[2].toString();
            const FileStorage fs(filename, FileStorage::READ);
            if (fs.isOpened()) {
                const FileNode& fn = fs["StatModel.EM"];
                obj.read(fn);
            } else {
                ostringstream emsg;
                emsg << "Could not open file " << filename << " for reading";
                mexErrMsgIdAndTxt("mexopencv:error",emsg.str().c_str());
            }
        } else {
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of arguments");
        }
    } else if (method == "predict") {
        if (nrhs != 3)
            mexErrMsgIdAndTxt("mexopencv:error","Wrong number of input arguments");
        Mat sample = rhs[2].toMat();
        Mat probs;
        Vec2d result = obj.predict(sample, probs);
        assign_lhs(result[0], result[1], probs);
    }
}
<|endoftext|>


<commit_before>#include "APICameraSystem.hpp"

using namespace kitrokopter;

/**
 * Constructs an APICameraSystem.
 */
APICameraSystem::APICameraSystem()
{

}

/**
 * Initialize the camera modules
 *
 * @param quadcopters The quadcopters which will be tracked.
 */
void APICameraSystem::initializeCameras(std::map<uint32_t, APIQuadcopter> quadcopters)
{
    camera_application::InitializeCameraService message = this->buildInitMessage(quadcopters);

    std::vector<uint32_t> cameraIds;
    for(std::map<uint32_t, APIQuadcopter>::const_iterator it = quadcopters.begin(); it != quadcopters.end(); ++it)
    {
        cameraIds.push_back(it->first);
    }

    std::stringstream sstm;
    std::stringstream err;

    for (int i = 0; i < cameraIds.size(); i++)
    {
	//empty the stringstream
	sstm.str("");
	sstm << "InitializeCameraService" << cameraIds[i];
	//copy the original message to be able to use the response
	camera_application::InitializeCameraService messageCopy = message;

	if (ros::service::call(sstm.str(), messageCopy))
        {
	    if (messageCopy.response.error != 0)
            {
		err << "InitializeCameraService returned error: " << messageCopy.response.error;
		throw new std::runtime_error(err.str());
	    }
	} else {
            throw new std::runtime_error("Could not call InitializeCameraService");
	}
    }
}

camera_application::InitializeCameraService APICameraSystem::buildInitMessage(std::map<uint32_t, APIQuadcopter> quadcopters)
{
    camera_application::InitializeCameraService message;
    message.request.header.stamp = ros::Time::now();

    std::vector<uint32_t> quadcopterIdsVector;
    MapKeysToVec(this->quadcopters, quadcopterIdsVector);
    message.request.quadCopterIds = &quadcopterIdsVector;

    uint32_t message.request.hsvColorRanges[quadcopterIdsVector.size()];
    for (int i = 0; i < quadcopterIdsVector.size(); i++) {
	message.request.hsvColorRangeshsvColorRanges[2*i] = this->quadcopters.get[i].getColorRange()[0];
	message.request.hsvColorRangeshsvColorRanges[2*i+1] = this->quadcopters.get[i].getColorRange()[1];
    }
    return message;
}

/**
 * Get a pointer to the APIQuadcopter with the corresponding id
 * or a null pointer if there is no such APIQuadcopter.
 *
 * @param id the APIQuadcopter's id
 * @return pointer to the APIQuadcopter with this id or a null pointer if there is no such APIQuadcopter
 */
APICamera * APICameraSystem::getCamera(int id)
{
    if (this->cameras.find(id) != this->cameras.end()) {
	return &cameras[id];
    } else {
	return NULL;
    }
}

/**
 * @return The number of cameras in this system.
 */
int APICameraSystem::getCameraAmount()
{
    return cameras.size();
}

/**
 * @return A vector with all cameras.
 */
std::vector<APICamera> APICameraSystem::getCameras()
{
    return cameras;
}

/**
 * Adds a camera to the camera system.
 * If there is already a camera with this id an exception will be thrown.
 *
 * @param camera the camera
 */
void APICameraSystem::addCamera(APICamera camera)
{
    if (this->cameras.find(camera.getId)) {
	throw new std::runtime_error("camera id already in use");
    } else {
	this->cameras.insert(std::pair<uint32_t,APICamera>(camera.getId,camera));
    }
}

/**
 * Finds all calibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getCalibratedCameras()
{
    std::vector<APICamera> result;
    for (std::map<uint32_t,APICamera>::iterator it = cameras.begin(); it != cameras.end(); ++it)
    {
	if (it->isCalibrated())
	{
	    result.push_back(&it->last);
	}
    }
    return result;
}

/**
 * Finds all uncalibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getUncalibratedCameras()
{
    std::vector<APICamera> result;
    for (std::map<uint32_t,APICamera>::iterator it = cameras.begin(); it != cameras.end(); ++it)
    {
	if (!it->isCalibrated())
	{
	    result.push_back(&it->last);
	}
    }
    return result;
}<commit_msg>Removed mapkeystovec<commit_after>#include "APICameraSystem.hpp"

using namespace kitrokopter;

/**
 * Constructs an APICameraSystem.
 */
APICameraSystem::APICameraSystem()
{

}

/**
 * Initialize the camera modules
 *
 * @param quadcopters The quadcopters which will be tracked.
 */
void APICameraSystem::initializeCameras(std::map<uint32_t, APIQuadcopter> quadcopters)
{
    camera_application::InitializeCameraService message = this->buildInitMessage(quadcopters);

    std::vector<uint32_t> cameraIds;
    for(std::map<uint32_t, APICamera>::const_iterator it = this->cameras.begin(); it != this->cameras.end(); ++it)
    {
        cameraIds.push_back(it->first);
    }

    std::stringstream sstm;
    std::stringstream err;

    for (int i = 0; i < cameraIds.size(); i++)
    {
	//empty the stringstream
	sstm.str("");
	sstm << "InitializeCameraService" << cameraIds[i];
	//copy the original message to be able to use the response
	camera_application::InitializeCameraService messageCopy = message;

	if (ros::service::call(sstm.str(), messageCopy))
        {
	    if (messageCopy.response.error != 0)
            {
		err << "InitializeCameraService returned error: " << messageCopy.response.error;
		throw new std::runtime_error(err.str());
	    }
	} else {
            throw new std::runtime_error("Could not call InitializeCameraService");
	}
    }
}

camera_application::InitializeCameraService APICameraSystem::buildInitMessage(std::map<uint32_t, APIQuadcopter> quadcopters)
{
    camera_application::InitializeCameraService message;
    message.request.header.stamp = ros::Time::now();

    std::vector<uint32_t> quadcopterIds;
    for(std::map<uint32_t, APIQuadcopter>::const_iterator it = quadcopters.begin(); it != quadcopters.end(); ++it)
    {
        quadcopterIds.push_back(it->first);
    }

    message.request.quadCopterIds = &quadcopterIdsVector;

    uint32_t message.request.hsvColorRanges[quadcopterIdsVector.size()];
    for (int i = 0; i < quadcopterIdsVector.size(); i++) {
	message.request.hsvColorRangeshsvColorRanges[2*i] = this->quadcopters.get[i].getColorRange()[0];
	message.request.hsvColorRangeshsvColorRanges[2*i+1] = this->quadcopters.get[i].getColorRange()[1];
    }
    return message;
}

/**
 * Get a pointer to the APIQuadcopter with the corresponding id
 * or a null pointer if there is no such APIQuadcopter.
 *
 * @param id the APIQuadcopter's id
 * @return pointer to the APIQuadcopter with this id or a null pointer if there is no such APIQuadcopter
 */
APICamera * APICameraSystem::getCamera(int id)
{
    if (this->cameras.find(id) != this->cameras.end()) {
	return &cameras[id];
    } else {
	return NULL;
    }
}

/**
 * @return The number of cameras in this system.
 */
int APICameraSystem::getCameraAmount()
{
    return cameras.size();
}

/**
 * @return A vector with all cameras.
 */
std::vector<APICamera> APICameraSystem::getCameras()
{
    return cameras;
}

/**
 * Adds a camera to the camera system.
 * If there is already a camera with this id an exception will be thrown.
 *
 * @param camera the camera
 */
void APICameraSystem::addCamera(APICamera camera)
{
    if (this->cameras.find(camera.getId)) {
	throw new std::runtime_error("camera id already in use");
    } else {
	this->cameras.insert(std::pair<uint32_t,APICamera>(camera.getId,camera));
    }
}

/**
 * Finds all calibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getCalibratedCameras()
{
    std::vector<APICamera> result;
    for (std::map<uint32_t,APICamera>::iterator it = cameras.begin(); it != cameras.end(); ++it)
    {
	if (it->isCalibrated())
	{
	    result.push_back(&it->last);
	}
    }
    return result;
}

/**
 * Finds all uncalibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getUncalibratedCameras()
{
    std::vector<APICamera> result;
    for (std::map<uint32_t,APICamera>::iterator it = cameras.begin(); it != cameras.end(); ++it)
    {
	if (!it->isCalibrated())
	{
	    result.push_back(&it->last);
	}
    }
    return result;
}<|endoftext|>


<commit_before>#include "ShapeAnalysisModuleCLP.h"
#include "ShapeAnalysisModuleComputation.h"
#include "Parameters.h"

using namespace std;
int main(int argc, char * argv [])
{
  PARSE_ARGS;
  cout << "------ Shape Analysis Module start -----" << endl;

  ShapeAnalysisModuleComputation m_computation;

  m_computation.SetModulePath(argv[0]);

  m_computation.SetChooseInputColumnState(inputColumn);
  if( inputColumn )
    {
    m_computation.SetColumnVolumeFile(columVolumeFile);
    }

  m_computation.SetParameterFile(GroupProjectInputFile.c_str() );
  m_computation.SetOutputDirectory(GroupProjectOutputDirectory.c_str() );
  m_computation.ReadFile(m_computation.GetParameterFile() );

  m_computation.SetOverwriteSegPostProcess(OverwriteSegPostProcess);
  m_computation.SetOverwriteGenParaMesh(OverwriteGenParaMesh);
  m_computation.SetOverwriteParaToSPHARMMesh(OverwriteParaToSPHARMMesh);
  m_computation.SetEnforcedSpace(sx, sy, sz);
  m_computation.SetLabelState(LabelState);
  m_computation.SetLabel(ValueLabelNumber);
  m_computation.SetNumberOfIterations(NumberofIterations);
  m_computation.SetSubdivLevel(SubdivLevelValue);
  m_computation.SetSPHARMDegree(SPHARMDegreeValue);
  m_computation.SetPhiIteration(phiIterationValue);
  m_computation.SetThetaIteration(thetaIterationValue);
  m_computation.SetMedialMesh(medialMesh);
  m_computation.SetRandomizeInputs(RandomizeInputs);
  m_computation.SetDebug(debug);

  m_computation.SetGaussianFilteringState(GaussianFiltering);
  if( GaussianFiltering )
    {
    m_computation.SetVarianceBox(VarianceX, VarianceY, VarianceZ);
    }

  m_computation.SetRegTemplateState(useRegTemplate);
  if( useRegTemplate )
    {
    m_computation.SetRegTemplate(regTemplate.c_str() );
    }
  m_computation.SetFlipTemplateState(useFlipTemplate);
  if( useFlipTemplate )
    {
    m_computation.SetFlipTemplate(flipTemplate.c_str());
    }

  m_computation.SetTemplateMState(MTemplate);
  m_computation.SetParaOut1State(ParaOut1Template);

  m_computation.SetHorizontalGridPara(HorizontalGridPara);
  m_computation.SetVerticalGridPara(VerticalGridPara);

  if( NoFLip )
    {
    m_computation.SetFinalFlip(1, 0, 0, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisX )
    {
    m_computation.SetFinalFlip(0, 1, 0, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisY )
    {
    m_computation.SetFinalFlip(0, 0, 1, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 1, 0, 0, 0, 0);
    }
  else if( FlipALongAxisXandY )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 1, 0, 0, 0);
    }
  else if( FlipALongAxisYandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 1, 0, 0);
    }
  else if( FlipALongAxisXandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 0, 1, 0);
    }
  else if( FlipALongAxisXandYandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 0, 0, 1);
    }

  m_computation.SetParticlesState(DoParticlesCorrespondence);

  m_computation.Computation();

  //return EXIT_SUCCESS;
  return -1;
}
<commit_msg>BUG: Fixed bad return values that made Slicer give a wrong completedWithErrors message<commit_after>#include "ShapeAnalysisModuleCLP.h"
#include "ShapeAnalysisModuleComputation.h"
#include "Parameters.h"

using namespace std;
int main(int argc, char * argv [])
{
  PARSE_ARGS;
  cout << "------ Shape Analysis Module start -----" << endl;

  ShapeAnalysisModuleComputation m_computation;

  m_computation.SetModulePath(argv[0]);

  m_computation.SetChooseInputColumnState(inputColumn);
  if( inputColumn )
    {
    m_computation.SetColumnVolumeFile(columVolumeFile);
    }

  m_computation.SetParameterFile(GroupProjectInputFile.c_str() );
  m_computation.SetOutputDirectory(GroupProjectOutputDirectory.c_str() );
  m_computation.ReadFile(m_computation.GetParameterFile() );

  m_computation.SetOverwriteSegPostProcess(OverwriteSegPostProcess);
  m_computation.SetOverwriteGenParaMesh(OverwriteGenParaMesh);
  m_computation.SetOverwriteParaToSPHARMMesh(OverwriteParaToSPHARMMesh);
  m_computation.SetEnforcedSpace(sx, sy, sz);
  m_computation.SetLabelState(LabelState);
  m_computation.SetLabel(ValueLabelNumber);
  m_computation.SetNumberOfIterations(NumberofIterations);
  m_computation.SetSubdivLevel(SubdivLevelValue);
  m_computation.SetSPHARMDegree(SPHARMDegreeValue);
  m_computation.SetPhiIteration(phiIterationValue);
  m_computation.SetThetaIteration(thetaIterationValue);
  m_computation.SetMedialMesh(medialMesh);
  m_computation.SetRandomizeInputs(RandomizeInputs);
  m_computation.SetDebug(debug);

  m_computation.SetGaussianFilteringState(GaussianFiltering);
  if( GaussianFiltering )
    {
    m_computation.SetVarianceBox(VarianceX, VarianceY, VarianceZ);
    }

  m_computation.SetRegTemplateState(useRegTemplate);
  if( useRegTemplate )
    {
    m_computation.SetRegTemplate(regTemplate.c_str() );
    }
  m_computation.SetFlipTemplateState(useFlipTemplate);
  if( useFlipTemplate )
    {
    m_computation.SetFlipTemplate(flipTemplate.c_str());
    }

  m_computation.SetTemplateMState(MTemplate);
  m_computation.SetParaOut1State(ParaOut1Template);

  m_computation.SetHorizontalGridPara(HorizontalGridPara);
  m_computation.SetVerticalGridPara(VerticalGridPara);

  if( NoFLip )
    {
    m_computation.SetFinalFlip(1, 0, 0, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisX )
    {
    m_computation.SetFinalFlip(0, 1, 0, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisY )
    {
    m_computation.SetFinalFlip(0, 0, 1, 0, 0, 0, 0, 0);
    }
  else if( FlipALongAxisZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 1, 0, 0, 0, 0);
    }
  else if( FlipALongAxisXandY )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 1, 0, 0, 0);
    }
  else if( FlipALongAxisYandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 1, 0, 0);
    }
  else if( FlipALongAxisXandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 0, 1, 0);
    }
  else if( FlipALongAxisXandYandZ )
    {
    m_computation.SetFinalFlip(0, 0, 0, 0, 0, 0, 0, 1);
    }

  m_computation.SetParticlesState(DoParticlesCorrespondence);

  m_computation.Computation();

  //return EXIT_SUCCESS;
  return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <stack>
#include <vector>
void getNeighborsAndPush(std::stack <int> &s, int node);

bool isVisited(std::vector<int> &visitedNode, int node);
void printVector(std::vector <int> &v);

int graph[][7]=
{
{0,1,1,1,0,0,0},
{1,0,0,0,0,1,0},
{1,0,0,1,1,0,1},
{1,0,1,0,1,0,0},
{0,0,1,1,0,0,1},
{0,1,0,0,0,0,0},
{0,0,1,0,1,0,0}
};

int main()
{
//Initialize a graph.

std::stack <int> s;
std::vector <int> nodes;
/*graph=
{
{0,1,1,1,0,0,0},
{1,0,0,0,0,1,0},
{1,0,0,1,1,0,1},
{1,0,1,0,1,0,0},
{0,0,1,1,0,0,1},
{0,1,0,0,0,0,0},
{0,0,1,0,1,0,0}
};
*/

int curNode= 0;
std::vector <int> visitedNode;

for(int i= 0; i < 7; i++)
{
	nodes.push_back(i);
}
	s.push( nodes[0] );

	while( !s.empty() )
	{
		curNode= s.top();
		s.pop();
		if (isVisited(visitedNode, curNode) )
		continue;
		else
		{
			getNeighborsAndPush(s, curNode);
			visitedNode.push_back(curNode);

		}
		//if the node: curNode is visited. continue
		// else  put its' all neighor nodes to stack and put it to visited[]
	}
	//std::cout<<"Size: "<< visitedNode.size();
printVector(visitedNode);
return 0;
}

void getNeighborsAndPush(std::stack <int> &s, int node)
{
	for(int i= 0; i < 7 ;i++)		//You should use vector instead of array.
	{
		if( graph[ node ][i]== 1) s.push( i );

	}

}
bool isVisited(std::vector<int> &visitedNode, int node)
{
	for(int i=0; i < visitedNode.size();i++)
	{
		if(node == visitedNode[i])
		return true;
	}
	return false;
}

void printVector(std::vector <int> &v)
{
	for(int i= 0; i < v.size(); ++i)
	{
		std::cout<<" "<< v[i];
	}

	std::cout<<'\n';
}
<commit_msg>Remove some shit<commit_after>#include <iostream>
#include <stack>
#include <vector>
void getNeighborsAndPush(std::stack <int> &s, int node);

bool isVisited(std::vector<int> &visitedNode, int node);
void printVector(std::vector <int> &v);

int graph[][7]=
{
{0,1,1,1,0,0,0},
{1,0,0,0,0,1,0},
{1,0,0,1,1,0,1},
{1,0,1,0,1,0,0},
{0,0,1,1,0,0,1},
{0,1,0,0,0,0,0},
{0,0,1,0,1,0,0}
};

int main()
{
std::stack <int> s;
std::vector <int> nodes;

int curNode= 0;
std::vector <int> visitedNode;

for(int i= 0; i < 7; i++)
{
	nodes.push_back(i);
}
	s.push( nodes[0] );

	while( !s.empty() )
	{
		curNode= s.top();
		s.pop();
		if (isVisited(visitedNode, curNode) )
		continue;
		else
		{
			getNeighborsAndPush(s, curNode);
			visitedNode.push_back(curNode);

		}
	}
printVector(visitedNode);
return 0;
}

void getNeighborsAndPush(std::stack <int> &s, int node)
{
	for(int i= 0; i < 7 ;i++)		//You should use vector instead of array.
	{
		if( graph[ node ][i]== 1) s.push( i );

	}
}

bool isVisited(std::vector<int> &visitedNode, int node)
{
	for(int i=0; i < visitedNode.size();i++)
	{
		if(node == visitedNode[i])
		return true;
	}
	return false;
}

void printVector(std::vector <int> &v)
{
	for(int i= 0; i < v.size(); ++i)
	{
		std::cout<<" "<< v[i];
	}

	std::cout<<'\n';
}
<|endoftext|>


<commit_before>#include "APICameraSystem.hpp"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

//Messages
#include "camera_application/InitializeCameraService.h"
#include "control_application/StartCalibration.h"
#include "control_application/TakeCalibrationPicture.h"
#include "control_application/CalculateCalibration.h"

using namespace kitrokopter;

/**
 * Constructs an APICameraSystem.
 */
APICameraSystem::APICameraSystem() {

}

/**
 * Initialize the camera modules
 *
 * @param quadcopters The quadcopters which will be tracked.
 */
void APICameraSystem::initializeCameras(
		std::map<uint32_t, APIQuadcopter> quadcopters) {
	camera_application::InitializeCameraService message =
			this->buildInitMessage(quadcopters);

	std::vector < uint32_t > cameraIds;
	for (std::map<uint32_t, APICamera>::const_iterator it =
			this->cameras.begin(); it != this->cameras.end(); ++it) {
		cameraIds.push_back(it->first);
	}

	std::stringstream sstm;
	std::stringstream err;

	for (int i = 0; i < cameraIds.size(); i++) {
		//empty the stringstream
		sstm.str("");
		sstm << "InitializeCameraService" << cameraIds[i];
		//copy the original message to be able to use the response
		camera_application::InitializeCameraService messageCopy = message;

		if (ros::service::call(sstm.str(), messageCopy)) {
			if (messageCopy.response.error != 0) {
				err << "InitializeCameraService returned error: "
						<< messageCopy.response.error;
				throw new std::runtime_error(err.str());
			}
		} else {
			throw new std::runtime_error(
					"Could not call InitializeCameraService");
		}
	}
}

/**
 * Create the message to initialize the cameras.
 *
 * @return the message
 */
camera_application::InitializeCameraService APICameraSystem::buildInitMessage(
		std::map<uint32_t, APIQuadcopter> quadcopters) {
	camera_application::InitializeCameraService message;
	message.request.header.stamp = ros::Time::now();

	std::vector < uint32_t > quadcopterIds;
	for (std::map<uint32_t, APIQuadcopter>::const_iterator it =
			quadcopters.begin(); it != quadcopters.end(); ++it) {
		quadcopterIds.push_back(it->first);
	}

	message.request.quadCopterIds = quadcopterIds;

	std::vector < uint32_t > colorRanges;
	for (int i = 0; i < quadcopterIds.size(); i++) {
		colorRanges.push_back(quadcopters[i].getColorRange()[0]);
		colorRanges.push_back(quadcopters[i].getColorRange()[1]);
	}
	message.request.hsvColorRanges = colorRanges;
	return message;
}

/**
 * Get a pointer to the APIQuadcopter with the corresponding id
 * or a null pointer if there is no such APIQuadcopter.
 *
 * @param id the APIQuadcopter's id
 * @return pointer to the APIQuadcopter with this id or a null pointer if there is no such APIQuadcopter
 */
APICamera * APICameraSystem::getCamera(int id) {
	if (this->cameras.find(id) != this->cameras.end()) {
		return &cameras[id];
	} else {
		return NULL;
	}
}

/**
 * @return The number of cameras in this system.
 */
int APICameraSystem::getCameraAmount() {
	return cameras.size();
}

/**
 * @return A pointer to the map of cameras.
 */
std::map<uint32_t, APICamera>* APICameraSystem::getCamerasAsMap() {
    return &this->cameras;
}

/**
 * @return A vector of pointers to the cameras.
 */
std::vector<APICamera*> APICameraSystem::getCamerasAsVector() {
    std::vector<APICamera*> result;
    for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
         it != cameras.end(); ++it) {
        result.push_back(&it->second);
    }
    return result;
}

/**
 * Adds a camera to the camera system.
 * If there is already a camera with this id an exception will be thrown.
 *
 * @param camera the camera
 */
void APICameraSystem::addCamera(APICamera camera) {
    if (this->cameras.find(camera.getId()) != this->cameras.end()) {
	throw new std::runtime_error("camera id already in use");
    } else {
	uint32_t id = camera.getId();
	this->cameras.insert(
		std::pair<uint32_t, APICamera>(id, camera));
	this->cameras[id].listen();
    }
}

/**
 * Finds all calibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getCalibratedCameras() {
	std::vector<APICamera*> result;
	for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
			it != cameras.end(); ++it) {
		if (it->second.isCalibrated()) {
			result.push_back(&it->second);
		}
	}
	return result;
}

bool APICameraSystem::startCalibration(const CalibrationBoard &board)
{
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::StartCalibration>("StartCalibration");
    control_application::StartCalibration srv;
    srv.request.chessboardWidth = board.horizontalNumber;
    srv.request.chessboardHeight = board.verticalNumber;
    srv.request.chessboardRealWidth = board.rectangleWidth;
    srv.request.chessboardRealHeight = board.rectangleHeight;
    if (client.call(srv)) {
	return srv.response.ok;
    } else {
	ROS_ERROR("Could not call StartCalibration.");
	return false;
    }
}

/**
 * Take a picture which will be used for calibration and
 * return the cameras' ids which took a picture wth a chessboard in it
 *
 * @return the ids of the cameras which contain a chessboard
 */
std::map<uint32_t, bool> APICameraSystem::takeCalibrationPictures() {
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::TakeCalibrationPicture>("TakeCalibrationPicture");
    control_application::TakeCalibrationPicture srv;
    if (client.call(srv))
    {
        cv_bridge::CvImagePtr cv_ptr;
        std::map<uint32_t, bool> result;
        if (srv.response.containsChessboard.size() != srv.response.ids.size()) {
            throw new std::runtime_error("Malformed take calibration picture answer. ContainsChessboard and ids must have the same length!");
        }
        for (int i = 0; i < srv.response.ids.size(); ++i)
        {
            result[srv.response.ids[i]] = srv.response.containsChessboard[i]
            auto cam = getCamera(srv.response.ids[i]);
            //ros provides functions to convert the images to a cv:Mat
            try
            {
                cv_ptr = cv_bridge::toCvCopy(srv.response.images[i], sensor_msgs::image_encodings::BGR8);
                cam->addCalibrationImage(cv_ptr->image);
            } catch (cv_bridge::Exception& e) {
                ROS_ERROR("cv_bridge exception: %s", e.what());
            }
        }
	return result;
    } else {
	ROS_ERROR("Could not call TakeCalibrationPicture.");
	return 0;
    }
}

/**
 * Send the signal to calculate the calibration after creating the calibration pictures
 * and retrieve the now calculated camera positions.
 */
void APICameraSystem::calculateCalibration()
{
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::CalculateCalibration>("CalculateCalibration");
    control_application::CalculateCalibration srv;
    if (client.call(srv)) {
	auto res = srv.response;
	uint32_t id;
	double x, y, z;
	for (int i = 0; i < res.IDs.size(); ++i) {
	    id = res.IDs[i];
	    x = res.cameraXPositions[i];
	    y = res.cameraYPositions[i];
	    z = res.cameraZPositions[i];
	    auto cam = getCamera(id);
	    if (cam) {
		cam->setPosition(x, y, z);
	    } else {
		ROS_ERROR("Calibrated invalid camera #%d.", (int)id);
	    }
	}
    } else {
	ROS_ERROR("Could not call CalculateCalibration.");
    }
}

/**
 * Finds all uncalibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getUncalibratedCameras() {
	std::vector<APICamera*> result;
	for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
			it != cameras.end(); ++it) {
		if (it->second.isCalibrated()) {
			result.push_back(&it->second);
		}
	}
	return result;
}
<commit_msg>Fix errors in APICameraSystem<commit_after>#include "APICameraSystem.hpp"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

//Messages
#include "camera_application/InitializeCameraService.h"
#include "control_application/StartCalibration.h"
#include "control_application/TakeCalibrationPicture.h"
#include "control_application/CalculateCalibration.h"

using namespace kitrokopter;

/**
 * Constructs an APICameraSystem.
 */
APICameraSystem::APICameraSystem() {

}

/**
 * Initialize the camera modules
 *
 * @param quadcopters The quadcopters which will be tracked.
 */
void APICameraSystem::initializeCameras(
		std::map<uint32_t, APIQuadcopter> quadcopters) {
	camera_application::InitializeCameraService message =
			this->buildInitMessage(quadcopters);

	std::vector < uint32_t > cameraIds;
	for (std::map<uint32_t, APICamera>::const_iterator it =
			this->cameras.begin(); it != this->cameras.end(); ++it) {
		cameraIds.push_back(it->first);
	}

	std::stringstream sstm;
	std::stringstream err;

	for (int i = 0; i < cameraIds.size(); i++) {
		//empty the stringstream
		sstm.str("");
		sstm << "InitializeCameraService" << cameraIds[i];
		//copy the original message to be able to use the response
		camera_application::InitializeCameraService messageCopy = message;

		if (ros::service::call(sstm.str(), messageCopy)) {
			if (messageCopy.response.error != 0) {
				err << "InitializeCameraService returned error: "
						<< messageCopy.response.error;
				throw new std::runtime_error(err.str());
			}
		} else {
			throw new std::runtime_error(
					"Could not call InitializeCameraService");
		}
	}
}

/**
 * Create the message to initialize the cameras.
 *
 * @return the message
 */
camera_application::InitializeCameraService APICameraSystem::buildInitMessage(
		std::map<uint32_t, APIQuadcopter> quadcopters) {
	camera_application::InitializeCameraService message;
	message.request.header.stamp = ros::Time::now();

	std::vector < uint32_t > quadcopterIds;
	for (std::map<uint32_t, APIQuadcopter>::const_iterator it =
			quadcopters.begin(); it != quadcopters.end(); ++it) {
		quadcopterIds.push_back(it->first);
	}

	message.request.quadCopterIds = quadcopterIds;

	std::vector < uint32_t > colorRanges;
	for (int i = 0; i < quadcopterIds.size(); i++) {
		colorRanges.push_back(quadcopters[i].getColorRange()[0]);
		colorRanges.push_back(quadcopters[i].getColorRange()[1]);
	}
	message.request.hsvColorRanges = colorRanges;
	return message;
}

/**
 * Get a pointer to the APIQuadcopter with the corresponding id
 * or a null pointer if there is no such APIQuadcopter.
 *
 * @param id the APIQuadcopter's id
 * @return pointer to the APIQuadcopter with this id or a null pointer if there is no such APIQuadcopter
 */
APICamera * APICameraSystem::getCamera(int id) {
	if (this->cameras.find(id) != this->cameras.end()) {
		return &cameras[id];
	} else {
		return NULL;
	}
}

/**
 * @return The number of cameras in this system.
 */
int APICameraSystem::getCameraAmount() {
	return cameras.size();
}

/**
 * @return A pointer to the map of cameras.
 */
std::map<uint32_t, APICamera>* APICameraSystem::getCamerasAsMap() {
    return &this->cameras;
}

/**
 * @return A vector of pointers to the cameras.
 */
std::vector<APICamera*> APICameraSystem::getCamerasAsVector() {
    std::vector<APICamera*> result;
    for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
         it != cameras.end(); ++it) {
        result.push_back(&it->second);
    }
    return result;
}

/**
 * Adds a camera to the camera system.
 * If there is already a camera with this id an exception will be thrown.
 *
 * @param camera the camera
 */
void APICameraSystem::addCamera(APICamera camera) {
    if (this->cameras.find(camera.getId()) != this->cameras.end()) {
	throw new std::runtime_error("camera id already in use");
    } else {
	uint32_t id = camera.getId();
	this->cameras.insert(
		std::pair<uint32_t, APICamera>(id, camera));
	this->cameras[id].listen();
    }
}

/**
 * Finds all calibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getCalibratedCameras() {
	std::vector<APICamera*> result;
	for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
			it != cameras.end(); ++it) {
		if (it->second.isCalibrated()) {
			result.push_back(&it->second);
		}
	}
	return result;
}

bool APICameraSystem::startCalibration(const CalibrationBoard &board)
{
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::StartCalibration>("StartCalibration");
    control_application::StartCalibration srv;
    srv.request.chessboardWidth = board.horizontalNumber;
    srv.request.chessboardHeight = board.verticalNumber;
    srv.request.chessboardRealWidth = board.rectangleWidth;
    srv.request.chessboardRealHeight = board.rectangleHeight;
    if (client.call(srv)) {
	return srv.response.ok;
    } else {
	ROS_ERROR("Could not call StartCalibration.");
	return false;
    }
}

/**
 * Take a picture which will be used for calibration and
 * return the cameras' ids which took a picture wth a chessboard in it
 *
 * @return the ids of the cameras which contain a chessboard
 */
std::map<uint32_t, bool> APICameraSystem::takeCalibrationPictures() {
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::TakeCalibrationPicture>("TakeCalibrationPicture");
    control_application::TakeCalibrationPicture srv;
    if (client.call(srv))
    {
        cv_bridge::CvImagePtr cv_ptr;
        std::map<uint32_t, bool> result;
        if (srv.response.containsChessboard.size() != srv.response.ids.size()) {
            throw new std::runtime_error("Malformed take calibration picture answer. ContainsChessboard and ids must have the same length!");
        }
        for (int i = 0; i < srv.response.ids.size(); ++i)
        {
            result[srv.response.ids[i]] = srv.response.containsChessboard[i];
            auto cam = getCamera(srv.response.ids[i]);
            //ros provides functions to convert the images to a cv:Mat
            try
            {
                cv_ptr = cv_bridge::toCvCopy(srv.response.images[i], sensor_msgs::image_encodings::BGR8);
                cam->addCalibrationImage(cv_ptr->image);
            } catch (cv_bridge::Exception& e) {
                ROS_ERROR("cv_bridge exception: %s", e.what());
            }
        }
	return result;
    } else {
	ROS_ERROR("Could not call TakeCalibrationPicture.");
	throw std::runtime_error("Could not call the TakeCalibrationPicture ROS service.");
    }
}

/**
 * Send the signal to calculate the calibration after creating the calibration pictures
 * and retrieve the now calculated camera positions.
 */
void APICameraSystem::calculateCalibration()
{
    ros::NodeHandle nodeHandle;
    ros::ServiceClient client = nodeHandle.serviceClient<control_application::CalculateCalibration>("CalculateCalibration");
    control_application::CalculateCalibration srv;
    if (client.call(srv)) {
	auto res = srv.response;
	uint32_t id;
	double x, y, z;
	for (int i = 0; i < res.IDs.size(); ++i) {
	    id = res.IDs[i];
	    x = res.cameraXPositions[i];
	    y = res.cameraYPositions[i];
	    z = res.cameraZPositions[i];
	    auto cam = getCamera(id);
	    if (cam) {
		cam->setPosition(x, y, z);
	    } else {
		ROS_ERROR("Calibrated invalid camera #%d.", (int)id);
	    }
	}
    } else {
	ROS_ERROR("Could not call CalculateCalibration.");
    }
}

/**
 * Finds all uncalibrated cameras.
 *
 * @return A vector of pointers to the  cameras.
 */
std::vector<APICamera*> APICameraSystem::getUncalibratedCameras() {
	std::vector<APICamera*> result;
	for (std::map<uint32_t, APICamera>::iterator it = cameras.begin();
			it != cameras.end(); ++it) {
		if (it->second.isCalibrated()) {
			result.push_back(&it->second);
		}
	}
	return result;
}
<|endoftext|>


<commit_before>#include "roman_numerals.h"

namespace
{

struct digit_values
{
    char numeral;
    int value;
};

const digit_values roman_numerals[] =
{
    { 'M', 1000 },
    { 'D', 500 },
    { 'C', 100 },
    { 'L', 50 },
    { 'X', 10 },
    { 'V', 5 },
    { 'I', 1 }
};

class converter
{
public:
    converter(int n)
        : n_(n)
    {}

    std::string convert();

private:
    void convert_thousands();
    void convert_hundreds();
    void convert_tens();
    void convert_ones();
    void convert_place(int place);
    void convert_place_numeral(int place);
    void convert_place_numeral_penultimate(int place, int penultimate_place);

    int n_;
    std::string result_;
};

std::string converter::convert()
{
    convert_thousands();
    convert_hundreds();
    convert_tens();
    convert_ones();
    return result_;
}

void converter::convert_thousands()
{
    convert_place(0);
}

void converter::convert_hundreds()
{
    convert_place(2);
}

void converter::convert_tens()
{
    convert_place(4);
}

void converter::convert_ones()
{
    result_ += std::string(n_, 'I');
}

void converter::convert_place(int place)
{
    const int tenth_place = place + 2;
    convert_place_numeral(place);
    convert_place_numeral_penultimate(place, tenth_place);
    const int half_place = place + 1;
    convert_place_numeral(half_place);
    convert_place_numeral_penultimate(half_place, tenth_place);
}

void converter::convert_place_numeral(int place)
{
    while (n_ >= roman_numerals[place].value) {
        result_ += roman_numerals[place].numeral;
        n_ -= roman_numerals[place].value;
    }
}

void converter::convert_place_numeral_penultimate(int place, int penultimate_place)
{
    if (n_ >= roman_numerals[place].value - roman_numerals[penultimate_place].value) {
        result_ += roman_numerals[penultimate_place].numeral;
        result_ += roman_numerals[place].numeral;
        n_ -= roman_numerals[place].value - roman_numerals[penultimate_place].value;
    }
}

}

std::string roman::convert(int n)
{
    return converter(n).convert();
}
<commit_msg>Refactor: Rename, Extract Variable<commit_after>#include "roman_numerals.h"

namespace
{

struct digit_values
{
    char numeral;
    int value;
};

const digit_values roman_numerals[] =
{
    { 'M', 1000 },
    { 'D', 500 },
    { 'C', 100 },
    { 'L', 50 },
    { 'X', 10 },
    { 'V', 5 },
    { 'I', 1 }
};

class converter
{
public:
    converter(int n)
        : n_(n)
    {}

    std::string convert();

private:
    void thousands();
    void hundreds();
    void tens();
    void ones();
    void place(int place);
    void place_numeral(int place);
    void place_numeral_penultimate(int place, int penultimate_place);

    int n_;
    std::string result_;
};

std::string converter::convert()
{
    thousands();
    hundreds();
    tens();
    ones();
    return result_;
}

void converter::thousands()
{
    place(0);
}

void converter::hundreds()
{
    place(2);
}

void converter::tens()
{
    place(4);
}

void converter::ones()
{
    result_ += std::string(n_, 'I');
}

void converter::place(int place)
{
    const int tenth_place = place + 2;
    place_numeral(place);
    place_numeral_penultimate(place, tenth_place);
    const int half_place = place + 1;
    place_numeral(half_place);
    place_numeral_penultimate(half_place, tenth_place);
}

void converter::place_numeral(int place)
{
    while (n_ >= roman_numerals[place].value) {
        result_ += roman_numerals[place].numeral;
        n_ -= roman_numerals[place].value;
    }
}

void converter::place_numeral_penultimate(int place, int penultimate_place)
{
    const int amount = roman_numerals[place].value - roman_numerals[penultimate_place].value;
    if (n_ >= amount) {
        result_ += roman_numerals[penultimate_place].numeral;
        result_ += roman_numerals[place].numeral;
        n_ -= amount;
    }
}

}

std::string roman::convert(int n)
{
    return converter(n).convert();
}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "AnnEventManager.hpp"
#include "AnnDefaultEventListener.hpp"
#include "AnnLogger.hpp"
#include "AnnEngine.hpp"
#include "AnnGetter.hpp"

using namespace Annwvyn;

AnnEventListener::AnnEventListener() :
	player(AnnGetPlayer().get())
{
}

float AnnEventListener::trim(float v, float dz)
{
	//The test is done on the abs value. Return the actual value, or 0 if under the dead-zone
	if (std::abs(v) >= dz) return v;
	return 0.0f;
}

std::shared_ptr<AnnEventListener> AnnEventListener::getSharedListener()
{
	return shared_from_this();
}

AnnTextInputer::AnnTextInputer() :
	listen(false),
	asciiOnly{ true }
{
}

bool AnnTextInputer::keyPressed(const OIS::KeyEvent &arg)
{
	if (!listen) return true;
	//If backspace, pop last char if possible
	if (arg.key == OIS::KC_BACK && !input.empty())
		input.pop_back();
	else if (arg.text < 127 && arg.text > 31 || arg.text == 13 || !asciiOnly)
		//Put typed char into the application
		input.push_back(char(arg.text));
	return true;
}

bool AnnTextInputer::keyReleased(const OIS::KeyEvent &arg)
{
	return true;
}

std::string AnnTextInputer::getInput() const
{
	return input;
}

void AnnTextInputer::clearInput()
{
	input.clear();
}

void AnnTextInputer::startListening()
{
	clearInput();
	listen = true;
}

void AnnTextInputer::stopListening()
{
	listen = false;
}

void AnnTextInputer::setInput(std::string content)
{
	input = content;
}

AnnEventManager::AnnEventManager(Ogre::RenderWindow* w) : AnnSubSystem("EventManager"),
Keyboard(nullptr),
Mouse(nullptr),
lastTimerCreated(0),
defaultEventListener(nullptr),
knowXbox(false),
keyboardIgnore{ false }
{
	//Reserve some memory
	keyEventBuffer.reserve(10);
	mouseEventBuffer.reserve(10);
	stickEventBuffer.reserve(10);
	handControllerEventBuffer.reserve(10);

	//Init all bool array to false
	for (auto& keyState : previousKeyStates) keyState = false;
	for (auto& mouseButtonState : previousMouseButtonStates) mouseButtonState = false;

	//Configure and create the input system
	size_t windowHnd;
	w->getCustomAttribute("WINDOW", &windowHnd);
	pl.insert(make_pair(std::string("WINDOW"), std::to_string(windowHnd)));
	InputManager = OIS::InputManager::createInputSystem(pl);

	//Get the keyboard, mouse and joysticks objects
	Keyboard = static_cast<OIS::Keyboard*>(InputManager->createInputObject(OIS::OISKeyboard, true));
	Mouse = static_cast<OIS::Mouse*>(InputManager->createInputObject(OIS::OISMouse, true));
	for (auto nbStick(0); nbStick < InputManager->getNumberOfDevices(OIS::OISJoyStick); nbStick++)
	{
		//Create joystick object
		const auto oisJoystick = static_cast<OIS::JoyStick*>(InputManager->createInputObject(OIS::OISJoyStick, true));
		Joysticks.emplace_back(oisJoystick);

		const auto& vendor = oisJoystick->vendor();
		AnnDebug() << "Detected joystick : " << vendor;

		//Test for the stick being an Xbox controller (Oculus, and PC in general uses Xbox as *standard* controller)
		if (vendor.find("Xbox") != std::string::npos ||
			vendor.find("XBOX") != std::string::npos)
		{
			knowXbox = true;
			xboxID = StickAxisId(oisJoystick->getID());
			AnnDebug() << "Detected Xbox controller at ID " << xboxID;
		}
	}

	textInputer = std::make_unique<AnnTextInputer>();
	Keyboard->setEventCallback(textInputer.get());
}

AnnTextInputer* AnnEventManager::getTextInputer() const
{
	return textInputer.get();
}

AnnEventManager::~AnnEventManager()
{
	clearListenerList();
	defaultEventListener = nullptr;
	Keyboard->setEventCallback(nullptr);
	delete Keyboard;
	delete Mouse;
	Joysticks.clear();
}

void AnnEventManager::useDefaultEventListener()
{
	AnnDebug("Reconfiguring the engine to use the default event listener");
	AnnDebug("This unregister any current listener in use!");

	//Remove all event listeners
	removeListener();

	//If the event listener isn't already initialized, allocate one
	if (!defaultEventListener)
		defaultEventListener = std::make_shared<AnnDefaultEventListener>();

	//Set the default event listener to the event manager
	addListener(defaultEventListener);
}

std::shared_ptr<AnnEventListener> AnnEventManager::getDefaultEventListener() const
{
	return defaultEventListener;
}

void AnnEventManager::addListener(std::shared_ptr<AnnEventListener> l)
{
	AnnDebug() << "Adding an event listener : " << l.get();
	if (l != nullptr)
		listeners.push_back(l);
}

void AnnEventManager::clearListenerList()
{
	listeners.clear();
}

//l equals NULL by default
void AnnEventManager::removeListener(std::shared_ptr<AnnEventListener> l)
{
	AnnDebug() << "Removing an event listener : " << l.get();
	if (l == nullptr) { clearListenerList(); return; }

	std::remove_if(begin(listeners), end(listeners),
		[&](std::weak_ptr<AnnEventListener> weak_listener)
	{
		if (auto listener = weak_listener.lock()) return listener == l;
		return false;
	});
}

void AnnEventManager::update()
{
	processCollisionEvents();
	processInput();
	processTriggerEvents();
	processTimers();
	processUserSpaceEvents();
}

unsigned int JoystickBuffer::idcounter = 0;

void AnnEventManager::captureEvents()
{
	//Capture events
	Keyboard->capture();
	Mouse->capture();

	for (auto& joystick : Joysticks)
		joystick.oisJoystick->capture();
}

void AnnEventManager::processKeyboardEvents()
{
	//for each key of the keyboard
	for (size_t c(0); c < KeyCode::SIZE; c++)
		if (Keyboard->isKeyDown(OIS::KeyCode(c)) && !previousKeyStates[c] ||
			!Keyboard->isKeyDown(OIS::KeyCode(c)) && previousKeyStates[c])
		{
			//create a corresponding key event
			AnnKeyEvent e;
			e.setCode(KeyCode::code(c));
			e.ignored = keyboardIgnore;
			bool(previousKeyStates[c] = Keyboard->isKeyDown(OIS::KeyCode(c))) ? e.setPressed() : e.setReleased();

			keyEventBuffer.push_back(e);
		}
}

void AnnEventManager::processMouseEvents()
{
	auto state(Mouse->getMouseState());

	AnnMouseEvent e;

	for (size_t i(0); i < nbButtons; i++)
		e.setButtonStatus(MouseButtonId(i), state.buttonDown(OIS::MouseButtonID(i)));

	e.setAxisInformation(X, AnnMouseAxis(X, state.X.rel, state.X.abs));
	e.setAxisInformation(Y, AnnMouseAxis(Y, state.Y.rel, state.Y.abs));
	e.setAxisInformation(Z, AnnMouseAxis(Z, state.Z.rel, state.Z.abs));

	mouseEventBuffer.push_back(e);
}

void AnnEventManager::processJoystickEvents()
{
	for (auto& Joystick : Joysticks)
	{
		const auto& state(Joystick.oisJoystick->getJoyStickState());
		AnnStickEvent stickEvent;

		//Get all buttons immediate data
		//stickEvent.buttons = state.mButtons;

		const auto buttonSize = state.mButtons.size();
		stickEvent.buttons.resize(buttonSize);
		for (auto i = 0u; i < buttonSize; ++i)
		{
			if (state.mButtons[i])
				stickEvent.buttons[i] = 1;
			else
				stickEvent.buttons[i] = 0;
		}

		stickEvent.vendor = Joystick.oisJoystick->vendor();
		stickEvent.stickID = Joystick.getID();

		//Get all axes immediate data
		auto axisID = 0;
		for (const auto& axis : state.mAxes)
		{
			AnnStickAxis annAxis{ axisID++, axis.rel, axis.abs };
			annAxis.noRel = axis.absOnly;
			stickEvent.axes.push_back(annAxis);
		}

		//The joystick state object always have 4 Pov but the AnnStickEvent has the number of Pov the stick has
		const auto nbPov = size_t(Joystick.oisJoystick->getNumberOfComponents(OIS::ComponentType::OIS_POV));
		for (auto i(0u); i < nbPov; i++)
			stickEvent.povs.push_back({ unsigned(state.mPOV[i].direction) });

		//Get press and release event lists
		const auto nbButton{ std::min(state.mButtons.size(), Joystick.previousStickButtonStates.size()) };
		for (auto button(0u); button < nbButton; button++)
			if (!Joystick.previousStickButtonStates[button] && state.mButtons[button])
				stickEvent.pressed.push_back(static_cast<unsigned short>(button));
			else if (Joystick.previousStickButtonStates[button] && !state.mButtons[button])
				stickEvent.released.push_back(static_cast<unsigned short>(button));

		//Save current buttons state for next frame
		Joystick.previousStickButtonStates = stickEvent.buttons;
		if (knowXbox) if (stickEvent.stickID == xboxID)
			stickEvent.xbox = true;

		stickEventBuffer.push_back(stickEvent);
	}
}

void AnnEventManager::processHandControllerEvents()
{
	if (AnnGetVRRenderer()->handControllersAvailable())
		for (auto handController : AnnGetVRRenderer()->getHandControllerArray())
		{
			if (!handController) continue;
			handControllerEventBuffer.push_back({ handController.get() });
		}
}

void AnnEventManager::pushEventsToListeners()
{
	for (auto& weak_listener : listeners) if (auto listener = weak_listener.lock())
	{
		for (auto& e : keyEventBuffer) listener->KeyEvent(e);
		for (auto& e : mouseEventBuffer) listener->MouseEvent(e);
		for (auto& e : stickEventBuffer) listener->StickEvent(e);
		for (auto& e : handControllerEventBuffer) listener->HandControllerEvent(e);

		listener->tick();
	}

	keyEventBuffer.clear();
	mouseEventBuffer.clear();
	stickEventBuffer.clear();
	handControllerEventBuffer.clear();
}

void AnnEventManager::processInput()
{
	captureEvents();
	processKeyboardEvents();
	processMouseEvents();
	processJoystickEvents();
	processHandControllerEvents();
	pushEventsToListeners();
}

timerID AnnEventManager::fireTimerMillisec(double delay)
{
	auto newID = lastTimerCreated++;
	futureTimers.push_back(AnnTimer(newID, delay));
	return newID;
}

timerID AnnEventManager::fireTimer(double delay)
{
	return fireTimerMillisec(1000 * delay);
}

void AnnEventManager::processTimers()
{
	//Append timers
	for (const auto& futureTimer : futureTimers)
		activeTimers.push_back(futureTimer);
	futureTimers.clear();

	//Send events
	for (auto weak_listener : listeners) if (auto listener = weak_listener.lock())
		for (const auto& timer : activeTimers)
			if (timer.isTimeout()) listener->TimeEvent({ timer });

	//Cleanup
	std::remove_if(begin(activeTimers), end(activeTimers),
		[&](const AnnTimer& timer) { return timer.isTimeout(); });
}

void AnnEventManager::processTriggerEvents()
{
	for (const auto& triggerEvent : triggerEventBuffer)
		for (auto weakListener : listeners) if (auto listener = weakListener.lock())
			listener->TriggerEvent(triggerEvent);

	triggerEventBuffer.clear();
}

void AnnEventManager::processCollisionEvents()
{
	for (auto weakListener : listeners) if (auto listener = weakListener.lock())
	{
		for (const auto& collisionBuffer : collisionBuffers)
		{
			const auto aMov = static_cast<AnnAbstractMovable*>(std::get<0>(collisionBuffer));
			const auto bMov = static_cast<AnnAbstractMovable*>(std::get<1>(collisionBuffer));
			const auto position = std::get<2>(collisionBuffer);
			const auto normal = std::get<3>(collisionBuffer);

			if (auto a = dynamic_cast<AnnGameObject*>(aMov)) if (auto b = dynamic_cast<AnnGameObject*>(bMov))
			{
				listener->CollisionEvent({ a, b, position, normal });
			}
		}

		for (auto playerCollision : playerCollisionBuffer)
			listener->PlayerCollisionEvent({ playerCollision });
	}

	collisionBuffers.clear();
	playerCollisionBuffer.clear();
}

size_t AnnEventManager::getNbStick() const
{
	return Joysticks.size();
}

AnnEventListener::~AnnEventListener()
{
}

void AnnEventManager::detectedCollision(void* a, void* b, AnnVect3 position, AnnVect3 normal)
{
	//The only body that doesn't have an "userPointer" set is the Player's rigidbody.
	//If one of the pair is null, it's a player collision that has been detected on this manifold.
	//Not an object-object collision
	if (!a) return playerCollision(b);
	if (!b) return playerCollision(a);

	//push the object-object collision in the buffer
	collisionBuffers.emplace_back(a, b, position, normal);
}

void AnnEventManager::playerCollision(void* object)
{
	auto movable = static_cast<AnnAbstractMovable*>(object);
	if (auto gameObject = dynamic_cast<AnnGameObject*>(movable))
	{
		playerCollisionBuffer.push_back(gameObject);
	}
	else if (auto triggerObject = dynamic_cast<AnnTriggerObject*>(movable))
	{
		AnnTriggerEvent e;
		e.sender = triggerObject;
		e.contact = true;
		triggerEventBuffer.push_back(e);
	}
}

void AnnEventManager::keyboardUsedForText(bool state)
{
	keyboardIgnore = state;
}

void AnnEventManager::userSpaceDispatchEvent(std::shared_ptr<AnnUserSpaceEvent> e, AnnUserSpaceEventLauncher* l)
{
	userSpaceEventBuffer.push_back(make_pair(e, l));
}

void AnnEventManager::processUserSpaceEvents()
{
	for (auto userSpaceEvent : userSpaceEventBuffer)
		for (auto weakListener : listeners) if (auto listener = weakListener.lock())
			listener->EventFromUserSubsystem(*userSpaceEvent.first, userSpaceEvent.second);
	userSpaceEventBuffer.clear();
}<commit_msg>fix usage of "remove_if". erase-remove idiom. :facepalm:<commit_after>#include "stdafx.h"
#include "AnnEventManager.hpp"
#include "AnnDefaultEventListener.hpp"
#include "AnnLogger.hpp"
#include "AnnEngine.hpp"
#include "AnnGetter.hpp"

using namespace Annwvyn;

AnnEventListener::AnnEventListener() :
	player(AnnGetPlayer().get())
{
}

float AnnEventListener::trim(float v, float dz)
{
	//The test is done on the abs value. Return the actual value, or 0 if under the dead-zone
	if (std::abs(v) >= dz) return v;
	return 0.0f;
}

std::shared_ptr<AnnEventListener> AnnEventListener::getSharedListener()
{
	return shared_from_this();
}

AnnTextInputer::AnnTextInputer() :
	listen(false),
	asciiOnly{ true }
{
}

bool AnnTextInputer::keyPressed(const OIS::KeyEvent &arg)
{
	if (!listen) return true;
	//If backspace, pop last char if possible
	if (arg.key == OIS::KC_BACK && !input.empty())
		input.pop_back();
	else if (arg.text < 127 && arg.text > 31 || arg.text == 13 || !asciiOnly)
		//Put typed char into the application
		input.push_back(char(arg.text));
	return true;
}

bool AnnTextInputer::keyReleased(const OIS::KeyEvent &arg)
{
	return true;
}

std::string AnnTextInputer::getInput() const
{
	return input;
}

void AnnTextInputer::clearInput()
{
	input.clear();
}

void AnnTextInputer::startListening()
{
	clearInput();
	listen = true;
}

void AnnTextInputer::stopListening()
{
	listen = false;
}

void AnnTextInputer::setInput(const std::string& content)
{
	input = content;
}

AnnEventManager::AnnEventManager(Ogre::RenderWindow* w) : AnnSubSystem("EventManager"),
Keyboard(nullptr),
Mouse(nullptr),
lastTimerCreated(0),
defaultEventListener(nullptr),
knowXbox(false),
keyboardIgnore{ false }
{
	//Reserve some memory
	keyEventBuffer.reserve(10);
	mouseEventBuffer.reserve(10);
	stickEventBuffer.reserve(10);
	handControllerEventBuffer.reserve(10);

	//Init all bool array to false
	for (auto& keyState : previousKeyStates) keyState = false;
	for (auto& mouseButtonState : previousMouseButtonStates) mouseButtonState = false;

	//Configure and create the input system
	size_t windowHnd;
	w->getCustomAttribute("WINDOW", &windowHnd);
	pl.insert(make_pair(std::string("WINDOW"), std::to_string(windowHnd)));
	InputManager = OIS::InputManager::createInputSystem(pl);

	//Get the keyboard, mouse and joysticks objects
	Keyboard = static_cast<OIS::Keyboard*>(InputManager->createInputObject(OIS::OISKeyboard, true));
	Mouse = static_cast<OIS::Mouse*>(InputManager->createInputObject(OIS::OISMouse, true));
	for (auto nbStick(0); nbStick < InputManager->getNumberOfDevices(OIS::OISJoyStick); nbStick++)
	{
		//Create joystick object
		const auto oisJoystick = static_cast<OIS::JoyStick*>(InputManager->createInputObject(OIS::OISJoyStick, true));
		Joysticks.emplace_back(oisJoystick);

		const auto& vendor = oisJoystick->vendor();
		AnnDebug() << "Detected joystick : " << vendor;

		//Test for the stick being an Xbox controller (Oculus, and PC in general uses Xbox as *standard* controller)
		if (vendor.find("Xbox") != std::string::npos ||
			vendor.find("XBOX") != std::string::npos)
		{
			knowXbox = true;
			xboxID = StickAxisId(oisJoystick->getID());
			AnnDebug() << "Detected Xbox controller at ID " << xboxID;
		}
	}

	textInputer = std::make_unique<AnnTextInputer>();
	Keyboard->setEventCallback(textInputer.get());
}

AnnTextInputer* AnnEventManager::getTextInputer() const
{
	return textInputer.get();
}

AnnEventManager::~AnnEventManager()
{
	clearListenerList();
	defaultEventListener = nullptr;
	Keyboard->setEventCallback(nullptr);
	delete Keyboard;
	delete Mouse;
	Joysticks.clear();
}

void AnnEventManager::useDefaultEventListener()
{
	AnnDebug("Reconfiguring the engine to use the default event listener");
	AnnDebug("This unregister any current listener in use!");

	//Remove all event listeners
	removeListener();

	//If the event listener isn't already initialized, allocate one
	if (!defaultEventListener)
		defaultEventListener = std::make_shared<AnnDefaultEventListener>();

	//Set the default event listener to the event manager
	addListener(defaultEventListener);
}

std::shared_ptr<AnnEventListener> AnnEventManager::getDefaultEventListener() const
{
	return defaultEventListener;
}

void AnnEventManager::addListener(std::shared_ptr<AnnEventListener> l)
{
	AnnDebug() << "Adding an event listener : " << l.get();
	if (l != nullptr)
		listeners.push_back(l);
}

void AnnEventManager::clearListenerList()
{
	listeners.clear();
}

//l equals NULL by default
void AnnEventManager::removeListener(std::shared_ptr<AnnEventListener> l)
{
	AnnDebug() << "Removing an event listener : " << l.get();
	if (l == nullptr) { clearListenerList(); return; }

	listeners.erase(std::remove_if(begin(listeners), end(listeners),
		[&](std::weak_ptr<AnnEventListener> weak_listener)
	{
		if (auto listener = weak_listener.lock()) return listener == l;
		return false;
	}), end(listeners));
}

void AnnEventManager::update()
{
	processCollisionEvents();
	processInput();
	processTriggerEvents();
	processTimers();
	processUserSpaceEvents();
}

unsigned int JoystickBuffer::idcounter = 0;

void AnnEventManager::captureEvents()
{
	//Capture events
	Keyboard->capture();
	Mouse->capture();

	for (auto& joystick : Joysticks)
		joystick.oisJoystick->capture();
}

void AnnEventManager::processKeyboardEvents()
{
	//for each key of the keyboard
	for (size_t c(0); c < KeyCode::SIZE; c++)
		if (Keyboard->isKeyDown(OIS::KeyCode(c)) && !previousKeyStates[c] ||
			!Keyboard->isKeyDown(OIS::KeyCode(c)) && previousKeyStates[c])
		{
			//create a corresponding key event
			AnnKeyEvent e;
			e.setCode(KeyCode::code(c));
			e.ignored = keyboardIgnore;
			bool(previousKeyStates[c] = Keyboard->isKeyDown(OIS::KeyCode(c))) ? e.setPressed() : e.setReleased();

			keyEventBuffer.push_back(e);
		}
}

void AnnEventManager::processMouseEvents()
{
	auto state(Mouse->getMouseState());

	AnnMouseEvent e;

	for (size_t i(0); i < nbButtons; i++)
		e.setButtonStatus(MouseButtonId(i), state.buttonDown(OIS::MouseButtonID(i)));

	e.setAxisInformation(X, AnnMouseAxis(X, state.X.rel, state.X.abs));
	e.setAxisInformation(Y, AnnMouseAxis(Y, state.Y.rel, state.Y.abs));
	e.setAxisInformation(Z, AnnMouseAxis(Z, state.Z.rel, state.Z.abs));

	mouseEventBuffer.push_back(e);
}

void AnnEventManager::processJoystickEvents()
{
	for (auto& Joystick : Joysticks)
	{
		const auto& state(Joystick.oisJoystick->getJoyStickState());
		AnnStickEvent stickEvent;

		//Get all buttons immediate data
		//stickEvent.buttons = state.mButtons;

		const auto buttonSize = state.mButtons.size();
		stickEvent.buttons.resize(buttonSize);
		for (auto i = 0u; i < buttonSize; ++i)
		{
			if (state.mButtons[i])
				stickEvent.buttons[i] = 1;
			else
				stickEvent.buttons[i] = 0;
		}

		stickEvent.vendor = Joystick.oisJoystick->vendor();
		stickEvent.stickID = Joystick.getID();

		//Get all axes immediate data
		auto axisID = 0;
		for (const auto& axis : state.mAxes)
		{
			AnnStickAxis annAxis{ axisID++, axis.rel, axis.abs };
			annAxis.noRel = axis.absOnly;
			stickEvent.axes.push_back(annAxis);
		}

		//The joystick state object always have 4 Pov but the AnnStickEvent has the number of Pov the stick has
		const auto nbPov = size_t(Joystick.oisJoystick->getNumberOfComponents(OIS::ComponentType::OIS_POV));
		for (auto i(0u); i < nbPov; i++)
			stickEvent.povs.push_back({ unsigned(state.mPOV[i].direction) });

		//Get press and release event lists
		const auto nbButton{ std::min(state.mButtons.size(), Joystick.previousStickButtonStates.size()) };
		for (auto button(0u); button < nbButton; button++)
			if (!Joystick.previousStickButtonStates[button] && state.mButtons[button])
				stickEvent.pressed.push_back(static_cast<unsigned short>(button));
			else if (Joystick.previousStickButtonStates[button] && !state.mButtons[button])
				stickEvent.released.push_back(static_cast<unsigned short>(button));

		//Save current buttons state for next frame
		Joystick.previousStickButtonStates = stickEvent.buttons;
		if (knowXbox) if (stickEvent.stickID == xboxID)
			stickEvent.xbox = true;

		stickEventBuffer.push_back(stickEvent);
	}
}

void AnnEventManager::processHandControllerEvents()
{
	if (AnnGetVRRenderer()->handControllersAvailable())
		for (auto handController : AnnGetVRRenderer()->getHandControllerArray())
		{
			if (!handController) continue;
			handControllerEventBuffer.push_back({ handController.get() });
		}
}

void AnnEventManager::pushEventsToListeners()
{
	for (auto& weak_listener : listeners) if (auto listener = weak_listener.lock())
	{
		for (auto& e : keyEventBuffer) listener->KeyEvent(e);
		for (auto& e : mouseEventBuffer) listener->MouseEvent(e);
		for (auto& e : stickEventBuffer) listener->StickEvent(e);
		for (auto& e : handControllerEventBuffer) listener->HandControllerEvent(e);

		listener->tick();
	}

	keyEventBuffer.clear();
	mouseEventBuffer.clear();
	stickEventBuffer.clear();
	handControllerEventBuffer.clear();
}

void AnnEventManager::processInput()
{
	captureEvents();
	processKeyboardEvents();
	processMouseEvents();
	processJoystickEvents();
	processHandControllerEvents();
	pushEventsToListeners();
}

timerID AnnEventManager::fireTimerMillisec(double delay)
{
	auto newID = lastTimerCreated++;
	futureTimers.push_back(AnnTimer(newID, delay));
	return newID;
}

timerID AnnEventManager::fireTimer(double delay)
{
	return fireTimerMillisec(1000 * delay);
}

void AnnEventManager::processTimers()
{
	//Append timers
	for (const auto& futureTimer : futureTimers)
		activeTimers.push_back(futureTimer);
	futureTimers.clear();

	//Send events
	for (auto weak_listener : listeners) if (auto listener = weak_listener.lock())
		for (const auto& timer : activeTimers)
			if (timer.isTimeout()) listener->TimeEvent({ timer });

	//Cleanup
	activeTimers.erase(std::remove_if(begin(activeTimers), end(activeTimers),
		[&](const AnnTimer& timer) { return timer.isTimeout(); }), end(activeTimers));
}

void AnnEventManager::processTriggerEvents()
{
	for (const auto& triggerEvent : triggerEventBuffer)
		for (auto weakListener : listeners) if (auto listener = weakListener.lock())
			listener->TriggerEvent(triggerEvent);

	triggerEventBuffer.clear();
}

void AnnEventManager::processCollisionEvents()
{
	for (auto weakListener : listeners) if (auto listener = weakListener.lock())
	{
		for (const auto& collisionBuffer : collisionBuffers)
		{
			const auto aMov = static_cast<AnnAbstractMovable*>(std::get<0>(collisionBuffer));
			const auto bMov = static_cast<AnnAbstractMovable*>(std::get<1>(collisionBuffer));
			const auto position = std::get<2>(collisionBuffer);
			const auto normal = std::get<3>(collisionBuffer);

			if (auto a = dynamic_cast<AnnGameObject*>(aMov)) if (auto b = dynamic_cast<AnnGameObject*>(bMov))
			{
				listener->CollisionEvent({ a, b, position, normal });
			}
		}

		for (auto playerCollision : playerCollisionBuffer)
			listener->PlayerCollisionEvent({ playerCollision });
	}

	collisionBuffers.clear();
	playerCollisionBuffer.clear();
}

size_t AnnEventManager::getNbStick() const
{
	return Joysticks.size();
}

AnnEventListener::~AnnEventListener()
{
}

void AnnEventManager::detectedCollision(void* a, void* b, AnnVect3 position, AnnVect3 normal)
{
	//The only body that doesn't have an "userPointer" set is the Player's rigidbody.
	//If one of the pair is null, it's a player collision that has been detected on this manifold.
	//Not an object-object collision
	if (!a) return playerCollision(b);
	if (!b) return playerCollision(a);

	//push the object-object collision in the buffer
	collisionBuffers.emplace_back(a, b, position, normal);
}

void AnnEventManager::playerCollision(void* object)
{
	auto movable = static_cast<AnnAbstractMovable*>(object);
	if (auto gameObject = dynamic_cast<AnnGameObject*>(movable))
	{
		playerCollisionBuffer.push_back(gameObject);
	}
	else if (auto triggerObject = dynamic_cast<AnnTriggerObject*>(movable))
	{
		AnnTriggerEvent e;
		e.sender = triggerObject;
		e.contact = true;
		triggerEventBuffer.push_back(e);
	}
}

void AnnEventManager::keyboardUsedForText(bool state)
{
	keyboardIgnore = state;
}

void AnnEventManager::userSpaceDispatchEvent(std::shared_ptr<AnnUserSpaceEvent> e, AnnUserSpaceEventLauncher* l)
{
	userSpaceEventBuffer.push_back(make_pair(e, l));
}

void AnnEventManager::processUserSpaceEvents()
{
	for (auto userSpaceEvent : userSpaceEventBuffer)
		for (auto weakListener : listeners) if (auto listener = weakListener.lock())
			listener->EventFromUserSubsystem(*userSpaceEvent.first, userSpaceEvent.second);
	userSpaceEventBuffer.clear();
}<|endoftext|>