<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 unscented_transform.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__FILTER__GAUSSIAN__UNSCENTED_TRANSFORM_HPP
#define FL__FILTER__GAUSSIAN__UNSCENTED_TRANSFORM_HPP

#include <fl/util/traits.hpp>
#include <fl/distribution/gaussian.hpp>
#include <fl/filter/gaussian/point_set_transform.hpp>

namespace fl
{

/**
 * \ingroup point_set_transform
 *
 * This is the Unscented Transform used in the Unscented Kalman Filter
 * \cite wan2000unscented . It implememnts the PointSetTransform interface.
 */
class UnscentedTransform
        : public PointSetTransform<UnscentedTransform>
{
public:
    /**
     * Creates a UnscentedTransform
     *
     * \param alpha     UT Scaling parameter alpha (distance to the mean)
     * \param beta      UT Scaling parameter beta  (2.0 is optimal for Gaussian)
     * \param kappa     UT Scaling parameter kappa (higher order parameter)
     */
    UnscentedTransform(double alpha = 1., double beta = 2., double kappa = 0.)
        : PointSetTransform<UnscentedTransform>(this),
          alpha_(alpha),
          beta_(beta),
          kappa_(kappa)
    { }

    /**
     * \copydoc PointSetTransform::forward(const Gaussian&,
     *                                     PointSet&) const
     *
     * \throws WrongSizeException
     * \throws ResizingFixedSizeEntityException
     */
    template <typename Gaussian_, typename PointSet_>
    void forward(const Gaussian_& gaussian,
                 PointSet_& point_set) const
    {
        forward(gaussian, gaussian.dimension(), 0, point_set);
    }

    /**
     * \copydoc PointSetTransform::forward(const Gaussian&,
     *                                     size_t global_dimension,
     *                                     size_t dimension_offset,
     *                                     PointSet&) const
     *
     * \throws WrongSizeException
     * \throws ResizingFixedSizeEntityException
     */
    template <typename Gaussian_, typename PointSet_>
    void forward(const Gaussian_& gaussian,
                 size_t global_dimension,
                 size_t dimension_offset,
                 PointSet_& point_set) const
    {
        typedef typename Traits<PointSet_>::Point  Point;
        typedef typename Traits<PointSet_>::Weight Weight;

        const double dim = double(global_dimension);
        const size_t point_count = number_of_points(dim);

        /**
         * \internal
         *
         * \remark
         * A PointSet with a fixed number of points must have the
         * correct number of points which is required by this transform
         */
        if (IsFixed<Traits<PointSet_>::NumberOfPoints>() &&
            Traits<PointSet_>::NumberOfPoints != point_count)
        {
            fl_throw(
                WrongSizeException("Incompatible number of points of the"
                                   " specified fixed-size PointSet"));
        }

        // will resize of transform size is different from point count.
        point_set.resize(point_count);

        auto&& covariance_sqrt = gaussian.square_root() * gamma_factor(dim);

        Point point_shift;
        const Point& mean = gaussian.mean();

        // set the first point
        point_set.point(0, mean, Weight{weight_mean_0(dim), weight_cov_0(dim)});

        // compute the remaining points
        Weight weight_i{weight_mean_i(dim), weight_cov_i(dim)};

        // use squential loops to enable loop unrolling
        const size_t start_1 = 1;
        const size_t limit_1 = start_1 + dimension_offset;
        const size_t limit_2 = limit_1 + gaussian.dimension();
        const size_t limit_3 = global_dimension;

        for (size_t i = start_1; i < limit_1; ++i)
        {
            point_set.point(i, mean, weight_i);
            point_set.point(global_dimension + i, mean, weight_i);
        }

        for (size_t i = limit_1; i < limit_2; ++i)
        {
            point_shift = covariance_sqrt.col(i - dimension_offset - 1);
            point_set.point(i, mean + point_shift, weight_i);
            point_set.point(global_dimension + i, mean - point_shift, weight_i);
        }

        for (size_t i = limit_2; i <= limit_3; ++i)
        {
            point_set.point(i, mean, weight_i);
            point_set.point(global_dimension + i, mean, weight_i);
        }
    }

    /**
     * \return Number of points generated by this transform
     *
     * \param dimension Dimension of the Gaussian
     */
    static constexpr size_t number_of_points(int dimension)
    {
        return (dimension != Eigen::Dynamic) ? 2 * dimension + 1 : -1;
    }

public:
    /** \cond INTERNAL */

    /**
     * \return First mean weight
     *
     * \param dim Dimension of the Gaussian
     */
    double weight_mean_0(double dim) const
    {
        return lambda_scalar(dim) / (dim + lambda_scalar(dim));
    }

    /**
     * \return First covariance weight
     *
     * \param dim Dimension of the Gaussian
     */
    double weight_cov_0(double dim) const
    {
        return weight_mean_0(dim) + (1 - alpha_ * alpha_ + beta_);
    }

    /**
     * \return i-th mean weight
     *
     * \param dimension Dimension of the Gaussian
     */
    double weight_mean_i(double dim) const
    {
        return 1. / (2. * (dim + lambda_scalar(dim)));
    }

    /**
     * \return i-th covariance weight
     *
     * \param dimension Dimension of the Gaussian
     */
    double weight_cov_i(double dim) const
    {
        return weight_mean_i(dim);
    }

    /**
     * \param dim Dimension of the Gaussian
     */
    double lambda_scalar(double dim) const
    {
        return alpha_ * alpha_ * (dim + kappa_) - dim;
    }

    /**
     * \param dim  Dimension of the Gaussian
     */
    double gamma_factor(double dim) const
    {
        return std::sqrt(dim + lambda_scalar(dim));
    }
    /** \endcond */

protected:
    /** \cond INTERNAL */
    double alpha_;
    double beta_;
    double kappa_;
    /** \endcond */
};

}

#endif
<commit_msg>number_of_points return type must be signed int<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 unscented_transform.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#ifndef FL__FILTER__GAUSSIAN__UNSCENTED_TRANSFORM_HPP
#define FL__FILTER__GAUSSIAN__UNSCENTED_TRANSFORM_HPP

#include <fl/util/traits.hpp>
#include <fl/distribution/gaussian.hpp>
#include <fl/filter/gaussian/point_set_transform.hpp>

namespace fl
{

/**
 * \ingroup point_set_transform
 *
 * This is the Unscented Transform used in the Unscented Kalman Filter
 * \cite wan2000unscented . It implememnts the PointSetTransform interface.
 */
class UnscentedTransform
        : public PointSetTransform<UnscentedTransform>
{
public:
    /**
     * Creates a UnscentedTransform
     *
     * \param alpha     UT Scaling parameter alpha (distance to the mean)
     * \param beta      UT Scaling parameter beta  (2.0 is optimal for Gaussian)
     * \param kappa     UT Scaling parameter kappa (higher order parameter)
     */
    UnscentedTransform(double alpha = 1., double beta = 2., double kappa = 0.)
        : PointSetTransform<UnscentedTransform>(this),
          alpha_(alpha),
          beta_(beta),
          kappa_(kappa)
    { }

    /**
     * \copydoc PointSetTransform::forward(const Gaussian&,
     *                                     PointSet&) const
     *
     * \throws WrongSizeException
     * \throws ResizingFixedSizeEntityException
     */
    template <typename Gaussian_, typename PointSet_>
    void forward(const Gaussian_& gaussian,
                 PointSet_& point_set) const
    {
        forward(gaussian, gaussian.dimension(), 0, point_set);
    }

    /**
     * \copydoc PointSetTransform::forward(const Gaussian&,
     *                                     size_t global_dimension,
     *                                     size_t dimension_offset,
     *                                     PointSet&) const
     *
     * \throws WrongSizeException
     * \throws ResizingFixedSizeEntityException
     */
    template <typename Gaussian_, typename PointSet_>
    void forward(const Gaussian_& gaussian,
                 size_t global_dimension,
                 size_t dimension_offset,
                 PointSet_& point_set) const
    {
        typedef typename Traits<PointSet_>::Point  Point;
        typedef typename Traits<PointSet_>::Weight Weight;

        const double dim = double(global_dimension);
        const size_t point_count = number_of_points(dim);

        /**
         * \internal
         *
         * \remark
         * A PointSet with a fixed number of points must have the
         * correct number of points which is required by this transform
         */
        if (IsFixed<Traits<PointSet_>::NumberOfPoints>() &&
            Traits<PointSet_>::NumberOfPoints != point_count)
        {
            fl_throw(
                WrongSizeException("Incompatible number of points of the"
                                   " specified fixed-size PointSet"));
        }

        // will resize of transform size is different from point count.
        point_set.resize(point_count);

        auto&& covariance_sqrt = gaussian.square_root() * gamma_factor(dim);

        Point point_shift;
        const Point& mean = gaussian.mean();

        // set the first point
        point_set.point(0, mean, Weight{weight_mean_0(dim), weight_cov_0(dim)});

        // compute the remaining points
        Weight weight_i{weight_mean_i(dim), weight_cov_i(dim)};

        // use squential loops to enable loop unrolling
        const size_t start_1 = 1;
        const size_t limit_1 = start_1 + dimension_offset;
        const size_t limit_2 = limit_1 + gaussian.dimension();
        const size_t limit_3 = global_dimension;

        for (size_t i = start_1; i < limit_1; ++i)
        {
            point_set.point(i, mean, weight_i);
            point_set.point(global_dimension + i, mean, weight_i);
        }

        for (size_t i = limit_1; i < limit_2; ++i)
        {
            point_shift = covariance_sqrt.col(i - dimension_offset - 1);
            point_set.point(i, mean + point_shift, weight_i);
            point_set.point(global_dimension + i, mean - point_shift, weight_i);
        }

        for (size_t i = limit_2; i <= limit_3; ++i)
        {
            point_set.point(i, mean, weight_i);
            point_set.point(global_dimension + i, mean, weight_i);
        }
    }

    /**
     * \return Number of points generated by this transform
     *
     * \param dimension Dimension of the Gaussian
     */
    static constexpr int number_of_points(int dimension)
    {
        return (dimension != Eigen::Dynamic) ? 2 * dimension + 1 : -1;
    }

public:
    /** \cond INTERNAL */

    /**
     * \return First mean weight
     *
     * \param dim Dimension of the Gaussian
     */
    double weight_mean_0(double dim) const
    {
        return lambda_scalar(dim) / (dim + lambda_scalar(dim));
    }

    /**
     * \return First covariance weight
     *
     * \param dim Dimension of the Gaussian
     */
    double weight_cov_0(double dim) const
    {
        return weight_mean_0(dim) + (1 - alpha_ * alpha_ + beta_);
    }

    /**
     * \return i-th mean weight
     *
     * \param dimension Dimension of the Gaussian
     */
    double weight_mean_i(double dim) const
    {
        return 1. / (2. * (dim + lambda_scalar(dim)));
    }

    /**
     * \return i-th covariance weight
     *
     * \param dimension Dimension of the Gaussian
     */
    double weight_cov_i(double dim) const
    {
        return weight_mean_i(dim);
    }

    /**
     * \param dim Dimension of the Gaussian
     */
    double lambda_scalar(double dim) const
    {
        return alpha_ * alpha_ * (dim + kappa_) - dim;
    }

    /**
     * \param dim  Dimension of the Gaussian
     */
    double gamma_factor(double dim) const
    {
        return std::sqrt(dim + lambda_scalar(dim));
    }
    /** \endcond */

protected:
    /** \cond INTERNAL */
    double alpha_;
    double beta_;
    double kappa_;
    /** \endcond */
};

}

#endif
<|endoftext|>


<commit_before>//===- Tiling.cpp - Implementation of linalg Tiling -----------------------===//
//
// Copyright 2019 The MLIR Authors.
//
// 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.
// =============================================================================
//
// This file implements the linalg dialect Tiling pass.
//
//===----------------------------------------------------------------------===//

#include "mlir/Dialect/Linalg/IR/LinalgOps.h"
#include "mlir/Dialect/Linalg/IR/LinalgTypes.h"
#include "mlir/Dialect/Linalg/Passes.h"
#include "mlir/Dialect/Linalg/Utils/Intrinsics.h"
#include "mlir/Dialect/Linalg/Utils/Utils.h"
#include "mlir/Dialect/LoopOps/LoopOps.h"
#include "mlir/EDSC/Helpers.h"
#include "mlir/IR/AffineExpr.h"
#include "mlir/IR/AffineExprVisitor.h"
#include "mlir/IR/AffineMap.h"
#include "mlir/IR/OpImplementation.h"
#include "mlir/Pass/Pass.h"
#include "mlir/Support/LLVM.h"
#include "mlir/Support/STLExtras.h"
#include "mlir/Transforms/FoldUtils.h"

#include "llvm/Support/CommandLine.h"

using namespace mlir;
using namespace mlir::edsc;
using namespace mlir::edsc::intrinsics;
using namespace mlir::linalg;
using namespace mlir::linalg::intrinsics;
using namespace mlir::loop;

#define DEBUG_TYPE "linalg-tiling"

static llvm::cl::OptionCategory clOptionsCategory(DEBUG_TYPE " options");
static llvm::cl::list<unsigned>
    clTileSizes("linalg-tile-sizes",
                llvm::cl::desc("Tile sizes by which to tile linalg operations"),
                llvm::cl::ZeroOrMore, llvm::cl::MiscFlags::CommaSeparated,
                llvm::cl::cat(clOptionsCategory));

static bool isZero(Value *v) {
  return isa_and_nonnull<ConstantIndexOp>(v->getDefiningOp()) &&
         cast<ConstantIndexOp>(v->getDefiningOp()).getValue() == 0;
}

// Creates a number of ranges equal to the number of non-zero in `tileSizes`.
// One for each loop of the LinalgOp that is tiled. The `tileSizes` argument has
// one entry per surrounding loop. It uses zero as the convention that a
// particular loop is not tiled. This convention simplifies implementations by
// avoiding affine map manipulations.
// The returned ranges correspond to the loop ranges, in the proper order, that
// are tiled and for which new loops will be created.
static SmallVector<SubViewOp::Range, 4>
makeTiledLoopRanges(OpBuilder &b, Location loc, AffineMap map,
                    ArrayRef<Value *> allViewSizes,
                    ArrayRef<Value *> allTileSizes, OperationFolder *folder) {
  assert(allTileSizes.size() == map.getNumResults());
  // Apply `map` to get view sizes in loop order.
  auto viewSizes = applyMapToValues(b, loc, map, allViewSizes, folder);
  SmallVector<Value *, 4> tileSizes(allTileSizes.begin(), allTileSizes.end());

  // Traverse the tile sizes, which are in loop order, erase zeros everywhere.
  for (int idx = tileSizes.size() - 1; idx >= 0; --idx) {
    if (isZero(tileSizes[idx])) {
      viewSizes.erase(viewSizes.begin() + idx);
      tileSizes.erase(tileSizes.begin() + idx);
    }
  }

  // Create a new range with the applied tile sizes.
  SmallVector<SubViewOp::Range, 4> res;
  for (unsigned idx = 0, e = tileSizes.size(); idx < e; ++idx) {
    res.push_back(SubViewOp::Range{constant_index(folder, 0), viewSizes[idx],
                                   tileSizes[idx]});
  }
  return res;
}

namespace {
// Helper visitor to determine whether an AffineExpr is tiled.
// This is achieved by traversing every AffineDimExpr with position `pos` and
// checking whether the corresponding `tileSizes[pos]` is non-zero.
// This also enforces only positive coefficients occur in multiplications.
//
// Example:
//   `d0 + 2 * d1 + d3` is tiled by [0, 0, 0, 2] but not by [0, 0, 2, 0]
//
struct TileCheck : public AffineExprVisitor<TileCheck> {
  TileCheck(ArrayRef<Value *> tileSizes)
      : isTiled(false), tileSizes(tileSizes) {}

  void visitDimExpr(AffineDimExpr expr) {
    isTiled |= !isZero(tileSizes[expr.getPosition()]);
  }
  void visitAffineBinaryOpExpr(AffineBinaryOpExpr expr) {
    visit(expr.getLHS());
    visit(expr.getRHS());
    if (expr.getKind() == mlir::AffineExprKind::Mul)
      assert(expr.getRHS().cast<AffineConstantExpr>().getValue() > 0 &&
             "nonpositive multiplying coefficient");
  }
  bool isTiled;
  ArrayRef<Value *> tileSizes;
};
} // namespace

static bool isTiled(AffineExpr expr, ArrayRef<Value *> tileSizes) {
  if (!expr)
    return false;
  TileCheck t(tileSizes);
  t.visit(expr);
  return t.isTiled;
}

// Checks whether the view with index `viewIndex` within `linalgOp` varies with
// respect to a non-zero `tileSize`.
static bool isTiled(AffineMap map, ArrayRef<Value *> tileSizes) {
  if (!map)
    return false;
  for (unsigned r = 0; r < map.getNumResults(); ++r)
    if (isTiled(map.getResult(r), tileSizes))
      return true;
  return false;
}

static SmallVector<Value *, 4>
makeTiledViews(OpBuilder &b, Location loc, LinalgOp linalgOp,
               ArrayRef<Value *> ivs, ArrayRef<Value *> tileSizes,
               ArrayRef<Value *> viewSizes, OperationFolder *folder) {
  assert(ivs.size() == static_cast<size_t>(llvm::count_if(
                           llvm::make_range(tileSizes.begin(), tileSizes.end()),
                           [](Value *v) { return !isZero(v); })) &&
         "expected as many ivs as non-zero sizes");

  using edsc::intrinsics::select;
  using edsc::op::operator+;
  using edsc::op::operator<;

  // Construct (potentially temporary) mins and maxes on which to apply maps
  // that define tile subviews.
  SmallVector<Value *, 8> lbs, subViewSizes;
  for (unsigned idx = 0, idxIvs = 0, e = tileSizes.size(); idx < e; ++idx) {
    bool isTiled = !isZero(tileSizes[idx]);
    lbs.push_back(isTiled ? ivs[idxIvs++] : (Value *)constant_index(folder, 0));
    subViewSizes.push_back(isTiled ? tileSizes[idx] : viewSizes[idx]);
  }

  auto *op = linalgOp.getOperation();

  SmallVector<Value *, 4> res;
  res.reserve(op->getNumOperands());
  auto viewIteratorBegin = linalgOp.getInputsAndOutputs().begin();
  for (unsigned viewIndex = 0; viewIndex < linalgOp.getNumInputsAndOutputs();
       ++viewIndex) {
    Value *view = *(viewIteratorBegin + viewIndex);
    unsigned rank = view->getType().cast<MemRefType>().getRank();
    auto map = loopToOperandRangesMaps(linalgOp)[viewIndex];
    // If the view is not tiled, we can use it as is.
    if (!isTiled(map, tileSizes)) {
      res.push_back(view);
      continue;
    }

    // Construct a new subview for the tile.
    SmallVector<Value *, 4> offsets, sizes, strides;
    offsets.reserve(rank);
    sizes.reserve(rank);
    strides.reserve(rank);
    for (unsigned r = 0; r < rank; ++r) {
      if (!isTiled(map.getSubMap({r}), tileSizes)) {
        offsets.push_back(constant_index(folder, 0));
        sizes.push_back(dim(view, r));
        strides.push_back(constant_index(folder, 1));
        continue;
      }

      // Tiling creates a new slice at the proper index, the slice step is 1
      // (i.e. the slice view does not subsample, stepping occurs in the loop).
      auto m = map.getSubMap({r});
      auto *offset = applyMapToValues(b, loc, m, lbs, folder).front();
      offsets.push_back(offset);
      auto *size = applyMapToValues(b, loc, m, subViewSizes, folder).front();
      sizes.push_back(size);
      strides.push_back(constant_index(folder, 1));
    }
    // TODO(b/144419024) Atm std.subview is not guaranteed in-bounds. Depending
    // on the semantics we attach to it, we may need to use min(size, dim) here
    // and canonicalize later.
    res.push_back(b.create<SubViewOp>(loc, view, offsets, sizes, strides));
  }

  // Traverse the mins/maxes and erase those that don't have uses left.
  // This is a special type of folding that we only apply when `folder` is
  // defined.
  if (folder) {
    lbs.append(subViewSizes.begin(), subViewSizes.end());
    for (auto *v : lbs)
      if (v->use_empty())
        v->getDefiningOp()->erase();
  }

  return res;
}

llvm::Optional<TiledLinalgOp>
mlir::linalg::tileLinalgOp(OpBuilder &b, LinalgOp op,
                           ArrayRef<Value *> tileSizes,
                           OperationFolder *folder) {
  // 1. Enforce the convention that "tiling by zero" skips tiling a particular
  // dimension. This convention is significantly simpler to handle instead of
  // adjusting affine maps to account for missing dimensions.
  assert(op.getNumParallelLoops() + op.getNumReductionLoops() +
                 op.getNumWindowLoops() ==
             tileSizes.size() &&
         "expected matching number of tile sizes and loops");
  OpBuilder::InsertionGuard g(b);
  b.setInsertionPoint(op);
  ScopedContext scope(b, op.getLoc());
  // 2. Build the tiled loop ranges.
  auto viewSizes = getViewSizes(op);
  // The flattened loopToOperandRangesMaps is expected to be an invertible
  // permutation map (asserted in the inverse calculation).
  auto viewSizesToLoopsMap =
      inversePermutation(concatAffineMaps(loopToOperandRangesMaps(op)));
  assert(viewSizesToLoopsMap && "expected invertible map");
  auto loopRanges =
      makeTiledLoopRanges(b, scope.getLocation(), viewSizesToLoopsMap,
                          viewSizes, tileSizes, folder);

  // 3. Create the tiled loops.
  LinalgOp res = op;
  SmallVector<IndexHandle, 4> ivs(loopRanges.size());
  auto pivs = makeIndexHandlePointers(ivs);
  LoopNestRangeBuilder(pivs, loopRanges)([&] {
    auto b = ScopedContext::getBuilder();
    auto loc = ScopedContext::getLocation();
    SmallVector<Value *, 4> ivValues(ivs.begin(), ivs.end());
    auto views =
        makeTiledViews(b, loc, op, ivValues, tileSizes, viewSizes, folder);
    auto operands = getAssumedNonViewOperands(op);
    views.append(operands.begin(), operands.end());
    res = op.clone(b, loc, views);
  });

  // 4. Gather the newly created loops and return them with the new op.
  SmallVector<ForOp, 8> loops;
  loops.reserve(ivs.size());
  for (auto iv : ivs)
    loops.push_back(loop::getForInductionVarOwner(iv));

  return TiledLinalgOp{res, loops};
}

llvm::Optional<TiledLinalgOp>
mlir::linalg::tileLinalgOp(OpBuilder &b, LinalgOp op,
                           ArrayRef<int64_t> tileSizes,
                           OperationFolder *folder) {
  if (tileSizes.empty())
    return llvm::None;

  // The following uses the convention that "tiling by zero" skips tiling a
  // particular dimension. This convention is significantly simpler to handle
  // instead of adjusting affine maps to account for missing dimensions.
  auto nLoops = op.getNumParallelLoops() + op.getNumReductionLoops() +
                op.getNumWindowLoops();
  tileSizes = tileSizes.take_front(nLoops);
  // If only 0 tilings are left, then return.
  if (llvm::all_of(tileSizes, [](int64_t v) { return v == 0; }))
    return llvm::None;

  // Create a builder for tile size constants.
  OpBuilder::InsertionGuard g(b);
  b.setInsertionPoint(op);
  ScopedContext scope(b, op.getLoc());

  // Materialize concrete tile size values to pass the generic tiling function.
  SmallVector<Value *, 8> tileSizeValues;
  tileSizeValues.reserve(tileSizes.size());
  for (auto ts : tileSizes)
    tileSizeValues.push_back(constant_index(folder, ts));
  // Pad tile sizes with zero values to enforce our convention.
  if (tileSizeValues.size() < nLoops) {
    for (unsigned i = tileSizeValues.size(); i < nLoops; ++i)
      tileSizeValues.push_back(constant_index(folder, 0));
  }

  return tileLinalgOp(b, op, tileSizeValues, folder);
}

static void tileLinalgOps(FuncOp f, ArrayRef<int64_t> tileSizes) {
  OpBuilder b(f);
  OperationFolder folder(f.getContext());
  f.walk([tileSizes, &b, &folder](LinalgOp op) {
    auto opLoopsPair = tileLinalgOp(b, op, tileSizes, &folder);
    // If tiling occurred successfully, erase old op.
    if (opLoopsPair)
      op.erase();
  });
  f.walk([](LinalgOp op) {
    if (!op.getOperation()->hasNoSideEffect())
      return;
    if (op.getOperation()->use_empty())
      op.erase();
  });
}

namespace {
struct LinalgTilingPass : public FunctionPass<LinalgTilingPass> {
  LinalgTilingPass() = default;
  LinalgTilingPass(ArrayRef<int64_t> sizes);

  void runOnFunction() override { tileLinalgOps(getFunction(), tileSizes); }

  SmallVector<int64_t, 8> tileSizes;
};
} // namespace

LinalgTilingPass::LinalgTilingPass(ArrayRef<int64_t> sizes) {
  this->tileSizes.assign(sizes.begin(), sizes.end());
}

std::unique_ptr<OpPassBase<FuncOp>>
mlir::linalg::createLinalgTilingPass(ArrayRef<int64_t> tileSizes) {
  return std::make_unique<LinalgTilingPass>(tileSizes);
}

static PassRegistration<LinalgTilingPass>
    pass("linalg-tile", "Tile operations in the linalg dialect", [] {
      auto pass = std::make_unique<LinalgTilingPass>();
      pass->tileSizes.assign(clTileSizes.begin(), clTileSizes.end());
      return pass;
    });
<commit_msg>Replace explicit concatenation by llvm::concat<commit_after>//===- Tiling.cpp - Implementation of linalg Tiling -----------------------===//
//
// Copyright 2019 The MLIR Authors.
//
// 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.
// =============================================================================
//
// This file implements the linalg dialect Tiling pass.
//
//===----------------------------------------------------------------------===//

#include "mlir/Dialect/Linalg/IR/LinalgOps.h"
#include "mlir/Dialect/Linalg/IR/LinalgTypes.h"
#include "mlir/Dialect/Linalg/Passes.h"
#include "mlir/Dialect/Linalg/Utils/Intrinsics.h"
#include "mlir/Dialect/Linalg/Utils/Utils.h"
#include "mlir/Dialect/LoopOps/LoopOps.h"
#include "mlir/EDSC/Helpers.h"
#include "mlir/IR/AffineExpr.h"
#include "mlir/IR/AffineExprVisitor.h"
#include "mlir/IR/AffineMap.h"
#include "mlir/IR/OpImplementation.h"
#include "mlir/Pass/Pass.h"
#include "mlir/Support/LLVM.h"
#include "mlir/Support/STLExtras.h"
#include "mlir/Transforms/FoldUtils.h"

#include "llvm/Support/CommandLine.h"

using namespace mlir;
using namespace mlir::edsc;
using namespace mlir::edsc::intrinsics;
using namespace mlir::linalg;
using namespace mlir::linalg::intrinsics;
using namespace mlir::loop;

#define DEBUG_TYPE "linalg-tiling"

static llvm::cl::OptionCategory clOptionsCategory(DEBUG_TYPE " options");
static llvm::cl::list<unsigned>
    clTileSizes("linalg-tile-sizes",
                llvm::cl::desc("Tile sizes by which to tile linalg operations"),
                llvm::cl::ZeroOrMore, llvm::cl::MiscFlags::CommaSeparated,
                llvm::cl::cat(clOptionsCategory));

static bool isZero(Value *v) {
  return isa_and_nonnull<ConstantIndexOp>(v->getDefiningOp()) &&
         cast<ConstantIndexOp>(v->getDefiningOp()).getValue() == 0;
}

// Creates a number of ranges equal to the number of non-zero in `tileSizes`.
// One for each loop of the LinalgOp that is tiled. The `tileSizes` argument has
// one entry per surrounding loop. It uses zero as the convention that a
// particular loop is not tiled. This convention simplifies implementations by
// avoiding affine map manipulations.
// The returned ranges correspond to the loop ranges, in the proper order, that
// are tiled and for which new loops will be created.
static SmallVector<SubViewOp::Range, 4>
makeTiledLoopRanges(OpBuilder &b, Location loc, AffineMap map,
                    ArrayRef<Value *> allViewSizes,
                    ArrayRef<Value *> allTileSizes, OperationFolder *folder) {
  assert(allTileSizes.size() == map.getNumResults());
  // Apply `map` to get view sizes in loop order.
  auto viewSizes = applyMapToValues(b, loc, map, allViewSizes, folder);
  SmallVector<Value *, 4> tileSizes(allTileSizes.begin(), allTileSizes.end());

  // Traverse the tile sizes, which are in loop order, erase zeros everywhere.
  for (int idx = tileSizes.size() - 1; idx >= 0; --idx) {
    if (isZero(tileSizes[idx])) {
      viewSizes.erase(viewSizes.begin() + idx);
      tileSizes.erase(tileSizes.begin() + idx);
    }
  }

  // Create a new range with the applied tile sizes.
  SmallVector<SubViewOp::Range, 4> res;
  for (unsigned idx = 0, e = tileSizes.size(); idx < e; ++idx) {
    res.push_back(SubViewOp::Range{constant_index(folder, 0), viewSizes[idx],
                                   tileSizes[idx]});
  }
  return res;
}

namespace {
// Helper visitor to determine whether an AffineExpr is tiled.
// This is achieved by traversing every AffineDimExpr with position `pos` and
// checking whether the corresponding `tileSizes[pos]` is non-zero.
// This also enforces only positive coefficients occur in multiplications.
//
// Example:
//   `d0 + 2 * d1 + d3` is tiled by [0, 0, 0, 2] but not by [0, 0, 2, 0]
//
struct TileCheck : public AffineExprVisitor<TileCheck> {
  TileCheck(ArrayRef<Value *> tileSizes)
      : isTiled(false), tileSizes(tileSizes) {}

  void visitDimExpr(AffineDimExpr expr) {
    isTiled |= !isZero(tileSizes[expr.getPosition()]);
  }
  void visitAffineBinaryOpExpr(AffineBinaryOpExpr expr) {
    visit(expr.getLHS());
    visit(expr.getRHS());
    if (expr.getKind() == mlir::AffineExprKind::Mul)
      assert(expr.getRHS().cast<AffineConstantExpr>().getValue() > 0 &&
             "nonpositive multiplying coefficient");
  }
  bool isTiled;
  ArrayRef<Value *> tileSizes;
};
} // namespace

static bool isTiled(AffineExpr expr, ArrayRef<Value *> tileSizes) {
  if (!expr)
    return false;
  TileCheck t(tileSizes);
  t.visit(expr);
  return t.isTiled;
}

// Checks whether the view with index `viewIndex` within `linalgOp` varies with
// respect to a non-zero `tileSize`.
static bool isTiled(AffineMap map, ArrayRef<Value *> tileSizes) {
  if (!map)
    return false;
  for (unsigned r = 0; r < map.getNumResults(); ++r)
    if (isTiled(map.getResult(r), tileSizes))
      return true;
  return false;
}

static SmallVector<Value *, 4>
makeTiledViews(OpBuilder &b, Location loc, LinalgOp linalgOp,
               ArrayRef<Value *> ivs, ArrayRef<Value *> tileSizes,
               ArrayRef<Value *> viewSizes, OperationFolder *folder) {
  assert(ivs.size() == static_cast<size_t>(llvm::count_if(
                           llvm::make_range(tileSizes.begin(), tileSizes.end()),
                           [](Value *v) { return !isZero(v); })) &&
         "expected as many ivs as non-zero sizes");

  using edsc::intrinsics::select;
  using edsc::op::operator+;
  using edsc::op::operator<;

  // Construct (potentially temporary) mins and maxes on which to apply maps
  // that define tile subviews.
  SmallVector<Value *, 8> lbs, subViewSizes;
  for (unsigned idx = 0, idxIvs = 0, e = tileSizes.size(); idx < e; ++idx) {
    bool isTiled = !isZero(tileSizes[idx]);
    lbs.push_back(isTiled ? ivs[idxIvs++] : (Value *)constant_index(folder, 0));
    subViewSizes.push_back(isTiled ? tileSizes[idx] : viewSizes[idx]);
  }

  auto *op = linalgOp.getOperation();

  SmallVector<Value *, 4> res;
  res.reserve(op->getNumOperands());
  auto viewIteratorBegin = linalgOp.getInputsAndOutputs().begin();
  for (unsigned viewIndex = 0; viewIndex < linalgOp.getNumInputsAndOutputs();
       ++viewIndex) {
    Value *view = *(viewIteratorBegin + viewIndex);
    unsigned rank = view->getType().cast<MemRefType>().getRank();
    auto map = loopToOperandRangesMaps(linalgOp)[viewIndex];
    // If the view is not tiled, we can use it as is.
    if (!isTiled(map, tileSizes)) {
      res.push_back(view);
      continue;
    }

    // Construct a new subview for the tile.
    SmallVector<Value *, 4> offsets, sizes, strides;
    offsets.reserve(rank);
    sizes.reserve(rank);
    strides.reserve(rank);
    for (unsigned r = 0; r < rank; ++r) {
      if (!isTiled(map.getSubMap({r}), tileSizes)) {
        offsets.push_back(constant_index(folder, 0));
        sizes.push_back(dim(view, r));
        strides.push_back(constant_index(folder, 1));
        continue;
      }

      // Tiling creates a new slice at the proper index, the slice step is 1
      // (i.e. the slice view does not subsample, stepping occurs in the loop).
      auto m = map.getSubMap({r});
      auto *offset = applyMapToValues(b, loc, m, lbs, folder).front();
      offsets.push_back(offset);
      auto *size = applyMapToValues(b, loc, m, subViewSizes, folder).front();
      sizes.push_back(size);
      strides.push_back(constant_index(folder, 1));
    }
    // TODO(b/144419024) Atm std.subview is not guaranteed in-bounds. Depending
    // on the semantics we attach to it, we may need to use min(size, dim) here
    // and canonicalize later.
    res.push_back(b.create<SubViewOp>(loc, view, offsets, sizes, strides));
  }

  // Traverse the mins/maxes and erase those that don't have uses left.
  // This is a special type of folding that we only apply when `folder` is
  // defined.
  if (folder)
    for (auto *v : llvm::concat<Value *>(lbs, subViewSizes))
      if (v->use_empty())
        v->getDefiningOp()->erase();

  return res;
}

llvm::Optional<TiledLinalgOp>
mlir::linalg::tileLinalgOp(OpBuilder &b, LinalgOp op,
                           ArrayRef<Value *> tileSizes,
                           OperationFolder *folder) {
  // 1. Enforce the convention that "tiling by zero" skips tiling a particular
  // dimension. This convention is significantly simpler to handle instead of
  // adjusting affine maps to account for missing dimensions.
  assert(op.getNumParallelLoops() + op.getNumReductionLoops() +
                 op.getNumWindowLoops() ==
             tileSizes.size() &&
         "expected matching number of tile sizes and loops");
  OpBuilder::InsertionGuard g(b);
  b.setInsertionPoint(op);
  ScopedContext scope(b, op.getLoc());
  // 2. Build the tiled loop ranges.
  auto viewSizes = getViewSizes(op);
  // The flattened loopToOperandRangesMaps is expected to be an invertible
  // permutation map (asserted in the inverse calculation).
  auto viewSizesToLoopsMap =
      inversePermutation(concatAffineMaps(loopToOperandRangesMaps(op)));
  assert(viewSizesToLoopsMap && "expected invertible map");
  auto loopRanges =
      makeTiledLoopRanges(b, scope.getLocation(), viewSizesToLoopsMap,
                          viewSizes, tileSizes, folder);

  // 3. Create the tiled loops.
  LinalgOp res = op;
  SmallVector<IndexHandle, 4> ivs(loopRanges.size());
  auto pivs = makeIndexHandlePointers(ivs);
  LoopNestRangeBuilder(pivs, loopRanges)([&] {
    auto b = ScopedContext::getBuilder();
    auto loc = ScopedContext::getLocation();
    SmallVector<Value *, 4> ivValues(ivs.begin(), ivs.end());
    auto views =
        makeTiledViews(b, loc, op, ivValues, tileSizes, viewSizes, folder);
    auto operands = getAssumedNonViewOperands(op);
    views.append(operands.begin(), operands.end());
    res = op.clone(b, loc, views);
  });

  // 4. Gather the newly created loops and return them with the new op.
  SmallVector<ForOp, 8> loops;
  loops.reserve(ivs.size());
  for (auto iv : ivs)
    loops.push_back(loop::getForInductionVarOwner(iv));

  return TiledLinalgOp{res, loops};
}

llvm::Optional<TiledLinalgOp>
mlir::linalg::tileLinalgOp(OpBuilder &b, LinalgOp op,
                           ArrayRef<int64_t> tileSizes,
                           OperationFolder *folder) {
  if (tileSizes.empty())
    return llvm::None;

  // The following uses the convention that "tiling by zero" skips tiling a
  // particular dimension. This convention is significantly simpler to handle
  // instead of adjusting affine maps to account for missing dimensions.
  auto nLoops = op.getNumParallelLoops() + op.getNumReductionLoops() +
                op.getNumWindowLoops();
  tileSizes = tileSizes.take_front(nLoops);
  // If only 0 tilings are left, then return.
  if (llvm::all_of(tileSizes, [](int64_t v) { return v == 0; }))
    return llvm::None;

  // Create a builder for tile size constants.
  OpBuilder::InsertionGuard g(b);
  b.setInsertionPoint(op);
  ScopedContext scope(b, op.getLoc());

  // Materialize concrete tile size values to pass the generic tiling function.
  SmallVector<Value *, 8> tileSizeValues;
  tileSizeValues.reserve(tileSizes.size());
  for (auto ts : tileSizes)
    tileSizeValues.push_back(constant_index(folder, ts));
  // Pad tile sizes with zero values to enforce our convention.
  if (tileSizeValues.size() < nLoops) {
    for (unsigned i = tileSizeValues.size(); i < nLoops; ++i)
      tileSizeValues.push_back(constant_index(folder, 0));
  }

  return tileLinalgOp(b, op, tileSizeValues, folder);
}

static void tileLinalgOps(FuncOp f, ArrayRef<int64_t> tileSizes) {
  OpBuilder b(f);
  OperationFolder folder(f.getContext());
  f.walk([tileSizes, &b, &folder](LinalgOp op) {
    auto opLoopsPair = tileLinalgOp(b, op, tileSizes, &folder);
    // If tiling occurred successfully, erase old op.
    if (opLoopsPair)
      op.erase();
  });
  f.walk([](LinalgOp op) {
    if (!op.getOperation()->hasNoSideEffect())
      return;
    if (op.getOperation()->use_empty())
      op.erase();
  });
}

namespace {
struct LinalgTilingPass : public FunctionPass<LinalgTilingPass> {
  LinalgTilingPass() = default;
  LinalgTilingPass(ArrayRef<int64_t> sizes);

  void runOnFunction() override { tileLinalgOps(getFunction(), tileSizes); }

  SmallVector<int64_t, 8> tileSizes;
};
} // namespace

LinalgTilingPass::LinalgTilingPass(ArrayRef<int64_t> sizes) {
  this->tileSizes.assign(sizes.begin(), sizes.end());
}

std::unique_ptr<OpPassBase<FuncOp>>
mlir::linalg::createLinalgTilingPass(ArrayRef<int64_t> tileSizes) {
  return std::make_unique<LinalgTilingPass>(tileSizes);
}

static PassRegistration<LinalgTilingPass>
    pass("linalg-tile", "Tile operations in the linalg dialect", [] {
      auto pass = std::make_unique<LinalgTilingPass>();
      pass->tileSizes.assign(clTileSizes.begin(), clTileSizes.end());
      return pass;
    });
<|endoftext|>


<commit_before>#include <gl/pogl.h>
#include <thread>
#include <atomic>
#include <vector>
#include <cmath>
#include "POGLExampleWindow.h"

static const POGL_CHAR DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS[] = { R"(
	#version 330

	layout(location = 0) in vec3 position;

	void main()
	{
		gl_Position = vec4(position, 1.0);
	}
)"};

static const POGL_CHAR DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS[] = { R"(
	#version 330

	// Output color for the render target location "0" defined when creating the framebuffer
	layout(location = 0) out vec4 texture0;

	// Output color for the render target location "1" defined when creating the framebuffer
	layout(location = 1) out vec4 texture1;

	void main()
	{
		// Draw red pixels to framebuffer location "0"
		texture0 = vec4(1, 0, 0, 1);

		// Draw blue pixels to framebuffer location "1"
		texture1 = vec4(0, 0, 1, 1);
	}
)" };

static const POGL_CHAR TEXTURING_VS[] = { R"(
	#version 330

	layout(location = 0) in vec3 position;
	layout(location = 2) in vec2 texCoord;

	out vec2 vs_TexCoord;

	void main()
	{
		vs_TexCoord = texCoord;
		gl_Position = vec4(position, 1.0);
	}
)" };

static const POGL_CHAR TEXTURING_FS[] = { R"(
	#version 330

	// Uniform texture
	uniform sampler2D Texture0;

	// Uniform texture
	uniform sampler2D Texture1;

	in vec2 vs_TexCoord;

	layout(location = 0) out vec4 color;

	void main()
	{
		color = texture(Texture0, vs_TexCoord) + texture(Texture1, vs_TexCoord);
	}
)" };

int main()
{
	// Create a window
	POGL_HANDLE windowHandle = POGLCreateExampleWindow(POGL_SIZEI(1024, 768), POGL_TOSTRING("Example: Framebuffers"));

	// Create a POGL device based on the supplied information
	POGL_DEVICE_INFO deviceInfo = { 0 };
#ifdef _DEBUG
	deviceInfo.flags = POGLDeviceInfoFlags::DEBUG_MODE;
#else
	deviceInfo.flags = POGLDeviceInfoFlags::DEBUG_MODE;
#endif
	deviceInfo.windowHandle = windowHandle;
	deviceInfo.colorBits = 32;
	deviceInfo.depthBits = 16;
	deviceInfo.pixelFormat = POGLPixelFormat::R8G8B8A8;
	IPOGLDevice* device = POGLCreateDevice(&deviceInfo);

	try {
		IPOGLDeviceContext* context = device->GetDeviceContext();

		//
		// Load the vertex- and fragment shader used to render to the framebuffer render targets
		//

		IPOGLShaderProgram* vertexShader = context->CreateShaderProgramFromMemory(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS,
			sizeof(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS), POGLShaderProgramType::VERTEX_SHADER);
		IPOGLShaderProgram* fragmentShader = context->CreateShaderProgramFromMemory(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS,
			sizeof(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS), POGLShaderProgramType::FRAGMENT_SHADER);
		IPOGLShaderProgram* programs[] = { vertexShader, fragmentShader, nullptr };
		IPOGLEffect* framebufferEffect = context->CreateEffectFromPrograms(programs);
		vertexShader->Release();
		fragmentShader->Release();

		//
		// Load the vertex- and fragment shader used to render the resulted framebuffer to the screen
		//

		vertexShader = context->CreateShaderProgramFromMemory(TEXTURING_VS, sizeof(TEXTURING_VS), POGLShaderProgramType::VERTEX_SHADER);
		fragmentShader = context->CreateShaderProgramFromMemory(TEXTURING_FS, sizeof(TEXTURING_FS), POGLShaderProgramType::FRAGMENT_SHADER);
		IPOGLShaderProgram* programs2[] = { vertexShader, fragmentShader, nullptr };
		IPOGLEffect* resultEffect = context->CreateEffectFromPrograms(programs2);
		vertexShader->Release();
		fragmentShader->Release();

		//
		// Create something to render to the framebuffer
		//

		const POGL_POSITION_VERTEX VERTICES[] = {
			POGL_POSITION_VERTEX(POGL_VECTOR3F(-0.5f, -0.5f, 0.0f)),
			POGL_POSITION_VERTEX(POGL_VECTOR3F(0.0f, 0.5f, 0.0f)),
			POGL_POSITION_VERTEX(POGL_VECTOR3F(0.5f, -0.5f, 0.0f))
		};
		IPOGLVertexBuffer* vertexBuffer = context->CreateVertexBuffer(VERTICES, sizeof(VERTICES), POGLPrimitiveType::TRIANGLE, POGLBufferUsage::STATIC);

		//
		// Create a fullscreen quad
		//

		const POGL_POSITION_TEXCOORD_VERTEX FULLSCREEN_VERTICES[] = {
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(-0.5f, -0.5f, 0.0f), POGL_VECTOR2F(0.0f, 0.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(-0.5f, 0.5f, 0.0f), POGL_VECTOR2F(0.0f, 1.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(0.5f, 0.5f, 0.0f), POGL_VECTOR2F(1.0f, 1.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(0.5f, -0.5f, 0.0f), POGL_VECTOR2F(1.0f, 0.0f))
		};
		IPOGLVertexBuffer* fullscreenVB = context->CreateVertexBuffer(FULLSCREEN_VERTICES, sizeof(FULLSCREEN_VERTICES), POGLPrimitiveType::TRIANGLE, POGLBufferUsage::STATIC);

		const POGL_UINT8 INDICES[] = {
			0, 1, 2,
			2, 3, 0
		};
		IPOGLIndexBuffer* fullscreenIB = context->CreateIndexBuffer(INDICES, sizeof(INDICES), POGLVertexType::UNSIGNED_BYTE, POGLBufferUsage::STATIC);

		//
		// Prepare and create a framebuffer to render to
		//

		IPOGLTexture2D* texture0 = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::RGBA32F, nullptr);
		IPOGLTexture2D* texture1 = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::RGBA32F, nullptr);
		IPOGLTexture2D* depthTexture = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::DEPTH24, nullptr);

		//
		// Create a framebuffer that renders to the following textures. The index in the
		// textures array defines the color location used in the fragment shader
		//

		IPOGLTexture* textures[] = { texture0, texture1, nullptr };
		IPOGLFramebuffer* framebuffer = context->CreateFramebuffer(textures, depthTexture);

		//
		// Release the textures. We don't really need them at this point anymore
		//

		texture0->Release(); texture1->Release(); depthTexture->Release();

		while (POGLProcessEvents()) {

			//
			// Bind the effect used when drawing to the framebuffer
			//

			IPOGLRenderState* state = context->Apply(framebufferEffect);
			state->SetFramebuffer(framebuffer);
			state->SetViewport(POGL_RECTI(0, 0, 1024, 768));
			state->Clear(POGLClearType::COLOR | POGLClearType::DEPTH);
			state->Draw(vertexBuffer);
			state->Release();

			//
			// Bind the effect used when drawing the textures generated by the effect above
			// and draw them on a fullscreen quad
			//

			state = context->Apply(resultEffect);
			state->SetFramebuffer(nullptr);
			state->Clear(POGLClearType::COLOR | POGLClearType::DEPTH);

			IPOGLTexture* texture0 = framebuffer->GetTexture(0);
			state->FindUniformByName("Texture0")->SetTexture(texture0);
			texture0->Release();

			IPOGLTexture* texture1 = framebuffer->GetTexture(1);
			state->FindUniformByName("Texture1")->SetTexture(texture1);
			texture1->Release();

			state->Draw(fullscreenVB, fullscreenIB);
			state->Release();

			//
			// End the frame and swap the display buffers
			//

			device->EndFrame();
		}

		framebuffer->Release();
		vertexBuffer->Release();
		fullscreenVB->Release();
		fullscreenIB->Release();
		resultEffect->Release();
		framebufferEffect->Release();
		context->Release();
	}
	catch (POGLException e) {
		POGLAlert(e);
	}

	device->Release();

	POGLDestroyExampleWindow(windowHandle);
	return 0;
}
<commit_msg>Updated comments<commit_after>#include <gl/pogl.h>
#include <thread>
#include <atomic>
#include <vector>
#include <cmath>
#include "POGLExampleWindow.h"

static const POGL_CHAR DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS[] = { R"(
	#version 330

	layout(location = 0) in vec3 position;

	void main()
	{
		gl_Position = vec4(position, 1.0);
	}
)"};

static const POGL_CHAR DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS[] = { R"(
	#version 330

	// Output color for the render target location "0" defined when creating the framebuffer
	layout(location = 0) out vec4 texture0;

	// Output color for the render target location "1" defined when creating the framebuffer
	layout(location = 1) out vec4 texture1;

	void main()
	{
		// Draw red pixels to framebuffer location "0"
		texture0 = vec4(1, 0, 0, 1);

		// Draw blue pixels to framebuffer location "1"
		texture1 = vec4(0, 0, 1, 1);
	}
)" };

static const POGL_CHAR TEXTURING_VS[] = { R"(
	#version 330

	layout(location = 0) in vec3 position;
	layout(location = 2) in vec2 texCoord;

	out vec2 vs_TexCoord;

	void main()
	{
		vs_TexCoord = texCoord;
		gl_Position = vec4(position, 1.0);
	}
)" };

static const POGL_CHAR TEXTURING_FS[] = { R"(
	#version 330

	// Uniform texture
	uniform sampler2D Texture0;

	// Uniform texture
	uniform sampler2D Texture1;

	in vec2 vs_TexCoord;

	layout(location = 0) out vec4 color;

	void main()
	{
		color = texture(Texture0, vs_TexCoord) + texture(Texture1, vs_TexCoord);
	}
)" };

int main()
{
	// Create a window
	POGL_HANDLE windowHandle = POGLCreateExampleWindow(POGL_SIZEI(1024, 768), POGL_TOSTRING("Example: Framebuffers"));

	// Create a POGL device based on the supplied information
	POGL_DEVICE_INFO deviceInfo = { 0 };
#ifdef _DEBUG
	deviceInfo.flags = POGLDeviceInfoFlags::DEBUG_MODE;
#else
	deviceInfo.flags = POGLDeviceInfoFlags::DEBUG_MODE;
#endif
	deviceInfo.windowHandle = windowHandle;
	deviceInfo.colorBits = 32;
	deviceInfo.depthBits = 16;
	deviceInfo.pixelFormat = POGLPixelFormat::R8G8B8A8;
	IPOGLDevice* device = POGLCreateDevice(&deviceInfo);

	try {
		IPOGLDeviceContext* context = device->GetDeviceContext();

		//
		// Load the vertex- and fragment shader used to render to the framebuffer render targets
		//

		IPOGLShaderProgram* vertexShader = context->CreateShaderProgramFromMemory(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS,
			sizeof(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_VS), POGLShaderProgramType::VERTEX_SHADER);
		IPOGLShaderProgram* fragmentShader = context->CreateShaderProgramFromMemory(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS,
			sizeof(DRAW_VERTICES_TO_FRAMEBUFFER_EFFECT_FS), POGLShaderProgramType::FRAGMENT_SHADER);
		IPOGLShaderProgram* programs[] = { vertexShader, fragmentShader, nullptr };
		IPOGLEffect* framebufferEffect = context->CreateEffectFromPrograms(programs);
		vertexShader->Release();
		fragmentShader->Release();

		//
		// Load the vertex- and fragment shader used to render the resulted framebuffer to the screen
		//

		vertexShader = context->CreateShaderProgramFromMemory(TEXTURING_VS, sizeof(TEXTURING_VS), POGLShaderProgramType::VERTEX_SHADER);
		fragmentShader = context->CreateShaderProgramFromMemory(TEXTURING_FS, sizeof(TEXTURING_FS), POGLShaderProgramType::FRAGMENT_SHADER);
		IPOGLShaderProgram* programs2[] = { vertexShader, fragmentShader, nullptr };
		IPOGLEffect* resultEffect = context->CreateEffectFromPrograms(programs2);
		vertexShader->Release();
		fragmentShader->Release();

		//
		// Create something to render to the framebuffer
		//

		const POGL_POSITION_VERTEX VERTICES[] = {
			POGL_POSITION_VERTEX(POGL_VECTOR3F(-0.5f, -0.5f, 0.0f)),
			POGL_POSITION_VERTEX(POGL_VECTOR3F(0.0f, 0.5f, 0.0f)),
			POGL_POSITION_VERTEX(POGL_VECTOR3F(0.5f, -0.5f, 0.0f))
		};
		IPOGLVertexBuffer* vertexBuffer = context->CreateVertexBuffer(VERTICES, sizeof(VERTICES), POGLPrimitiveType::TRIANGLE, POGLBufferUsage::STATIC);

		//
		// Create a fullscreen quad
		//

		const POGL_POSITION_TEXCOORD_VERTEX FULLSCREEN_VERTICES[] = {
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(-0.5f, -0.5f, 0.0f), POGL_VECTOR2F(0.0f, 0.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(-0.5f, 0.5f, 0.0f), POGL_VECTOR2F(0.0f, 1.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(0.5f, 0.5f, 0.0f), POGL_VECTOR2F(1.0f, 1.0f)),
			POGL_POSITION_TEXCOORD_VERTEX(POGL_VECTOR3F(0.5f, -0.5f, 0.0f), POGL_VECTOR2F(1.0f, 0.0f))
		};
		IPOGLVertexBuffer* fullscreenVB = context->CreateVertexBuffer(FULLSCREEN_VERTICES, sizeof(FULLSCREEN_VERTICES), POGLPrimitiveType::TRIANGLE, POGLBufferUsage::STATIC);

		const POGL_UINT8 INDICES[] = {
			0, 1, 2,
			2, 3, 0
		};
		IPOGLIndexBuffer* fullscreenIB = context->CreateIndexBuffer(INDICES, sizeof(INDICES), POGLVertexType::UNSIGNED_BYTE, POGLBufferUsage::STATIC);

		//
		// Prepare and create a framebuffer to render to
		//

		IPOGLTexture2D* texture0 = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::RGBA32F, nullptr);
		IPOGLTexture2D* texture1 = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::RGBA32F, nullptr);
		IPOGLTexture2D* depthTexture = context->CreateTexture2D(POGL_SIZEI(1024, 768), POGLTextureFormat::DEPTH24, nullptr);

		//
		// Create a framebuffer that renders to the following textures. The index in the
		// textures array defines the color location used in the fragment shader
		//

		IPOGLTexture* textures[] = { texture0, texture1, nullptr };
		IPOGLFramebuffer* framebuffer = context->CreateFramebuffer(textures, depthTexture);

		//
		// Release the depth texture. We don't really need it anymore
		//

		depthTexture->Release();

		while (POGLProcessEvents()) {
			IPOGLRenderState* state = context->Apply(framebufferEffect);

			//
			// Set the framebuffer to the render state. This enables us to render to the attached textures
			// This also changes the viewport of the IPOGLRenderState.
			//

			state->SetFramebuffer(framebuffer);

			//
			// Update the viewport based on the framebuffer
			//

			state->SetViewport(POGL_RECTI(0, 0, 1024, 768));

			//
			// Render as usual
			//

			state->Clear(POGLClearType::COLOR | POGLClearType::DEPTH);
			state->Draw(vertexBuffer);
			state->Release();

			//
			// Bind the effect used when drawing the textures generated by the effect above
			// and draw them on a fullscreen quad
			//

			state = context->Apply(resultEffect);

			//
			// Detach the active framebuffer so that we render to the screen instead of to the textures
			//

			state->SetFramebuffer(nullptr);
			state->Clear(POGLClearType::COLOR | POGLClearType::DEPTH);

			//
			// Bind the textures to the uniforms "Texture0" and "Texture1".
			// You can also get the textures by calling IPOGLFramebuffer::GetTexture(POGL_UINT32).
			//

			state->FindUniformByName("Texture0")->SetTexture(texture0);
			state->FindUniformByName("Texture1")->SetTexture(texture1);

			state->Draw(fullscreenVB, fullscreenIB);
			state->Release();

			//
			// End the frame and swap the display buffers
			//

			device->EndFrame();
		}

		texture0->Release();
		texture1->Release();
		framebuffer->Release();
		vertexBuffer->Release();
		fullscreenVB->Release();
		fullscreenIB->Release();
		resultEffect->Release();
		framebufferEffect->Release();
		context->Release();
	}
	catch (POGLException e) {
		POGLAlert(e);
	}

	device->Release();

	POGLDestroyExampleWindow(windowHandle);
	return 0;
}
<|endoftext|>


<commit_before>/*************************************************************************
This software allows for filtering in high-dimensional measurement and
state spaces, as described in

M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal.
Probabilistic Object Tracking using a Range Camera
IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013

In a publication based on this software pleace cite the above reference.

Copyright (C) 2014  Manuel Wuthrich

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*************************************************************************/

#ifndef TRACKING_DATASET_HPP_
#define TRACKING_DATASET_HPP_

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <fstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>

class DataFrame
{
public:
    sensor_msgs::Image::ConstPtr image_;
    sensor_msgs::CameraInfo::ConstPtr info_;
    Eigen::VectorXd ground_truth_;

    DataFrame(const sensor_msgs::Image::ConstPtr& image,
              const sensor_msgs::CameraInfo::ConstPtr& info,
              const Eigen::VectorXd& ground_truth = Eigen::VectorXd()):
        image_(image),
        info_(info),
        ground_truth_(ground_truth){ }
};

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
    void newMessage(const boost::shared_ptr<M const> &msg)
    {
        signalMessage(msg);
    }
};

class TrackingDataset
{
public:
    TrackingDataset(const string& path):path_(path),
                                image_topic_("XTION/depth/image"),
                                info_topic_("XTION/depth/camera_info"),
                                measurements_filename_("measurements.bag"),
                                ground_truth_filename_("ground_truth.txt"),
                                admissible_delta_time_(0.0001) {}
    ~TrackingDataset() {}

    void addFrame(const sensor_msgs::Image::ConstPtr& image,
                  const sensor_msgs::CameraInfo::ConstPtr& info,
                   const Eigen::VectorXd& ground_truth = Eigen::VectorXd())
    {
        DataFrame data(image, info, ground_truth);
        data_.push_back(data);
    }

    void addFrame(const sensor_msgs::Image::ConstPtr& image,
                  const sensor_msgs::CameraInfo::ConstPtr& info)
    {
        DataFrame data(image, info);
        data_.push_back(data);
    }

    sensor_msgs::Image::ConstPtr getImage(const size_t& index)
    {
        return data_[index].image_;
    }

    sensor_msgs::CameraInfo::ConstPtr getInfo(const size_t& index)
    {
        return data_[index].info_;
    }
    Eigen::Matrix3d getCameraMatrix(const size_t& index)
    {
        Matrix3d camera_matrix;
        for(size_t col = 0; col < 3; col++)
            for(size_t row = 0; row < 3; row++)
                camera_matrix(row,col) = data_[0].info_->K[col+row*3];
        return camera_matrix;
    }

    Eigen::VectorXd getGroundTruth(const size_t& index)
    {
        return data_[index].ground_truth_;
    }

    size_t sIze()
    {
        return data_.size();
    }

    void loAd()
    {
        // load bagfile ----------------------------------------------------------------------------
        rosbag::Bag bag;
        bag.open((path_ / measurements_filename_).string(), rosbag::bagmode::Read);

        // Image topics to load
        std::vector<std::string> topics;
        topics.push_back(image_topic_);
        topics.push_back(info_topic_);
        rosbag::View view(bag, rosbag::TopicQuery(topics));

        // Set up fake subscribers to capture images
        BagSubscriber<sensor_msgs::Image> image_subscriber;
        BagSubscriber<sensor_msgs::CameraInfo> info_subscriber;

        // Use time synchronizer to make sure we get properly synchronized images
        message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo>
                sync(image_subscriber, info_subscriber, 25);
        sync.registerCallback(boost::bind(&TrackingDataset::addFrame, this,  _1, _2));

        // Load all messages into our stereo TrackingDataset
        BOOST_FOREACH(rosbag::MessageInstance const m, view)
        {
            if (m.getTopic() == image_topic_ || ("/" + m.getTopic() == image_topic_))
            {
                sensor_msgs::Image::ConstPtr image = m.instantiate<sensor_msgs::Image>();
                if (image != NULL)
                    image_subscriber.newMessage(image);
            }

            if (m.getTopic() == info_topic_ || ("/" + m.getTopic() == info_topic_))
            {
                sensor_msgs::CameraInfo::ConstPtr info = m.instantiate<sensor_msgs::CameraInfo>();
                if (info != NULL)
                    info_subscriber.newMessage(info);
            }
        }
        bag.close();

        // load ground_truth.txt ---------------------------------------------------------------------
        ifstream file; file.open((path_ / ground_truth_filename_).c_str(), ios::in); // open file
        if(file.is_open())
        {
            std::string temp; std::getline(file, temp); std::istringstream line(temp); // get a line from file
            double time_stamp; line >> time_stamp; // get the timestamp
            double scalar; VectorXd state;
            while(line >> scalar) // get the state
            {
                VectorXd temp(state.rows() + 1);
                temp.topRows(state.rows()) = state;
                temp(temp.rows()-1) = scalar;
                state = temp;
            }
            file.close();

            // attach the state to the appropriate data frames
            for(size_t i = 0; i < data_.size(); i++)
                if(std::fabs(data_[i].image_->header.stamp.toSec() - time_stamp) <= admissible_delta_time_)
                    data_[i].ground_truth_ = state;
        }
        else
        {
            cout << "could not open file " << path_ / ground_truth_filename_ << endl;
            exit(-1);
        }
    }

    void stOre()
    {
        if(boost::filesystem::exists(path_))
        {
            std::cout << "TrackingDataset with name " << path_ << " already exists, will not overwrite." << endl;
            return;
        }
        else
            boost::filesystem::create_directory(path_);

        // write images to bagfile -----------------------------------------------------------------
        rosbag::Bag bag;
        bag.open((path_ / measurements_filename_).string(), rosbag::bagmode::Write);

        std::vector<std::string> topics;
        topics.push_back(image_topic_);
        topics.push_back(info_topic_);

        for(size_t i = 0; i < data_.size(); i++)
        {
            bag.write(image_topic_, data_[i].image_->header.stamp, data_[i].image_);
            bag.write(info_topic_, data_[i].info_->header.stamp, data_[i].info_);
        }
        bag.close();

        // write ground truth to txt file ----------------------------------------------------------
        ofstream file; file.open((path_ / ground_truth_filename_).c_str(), ios::out | ios::trunc);
        if(file.is_open())
        {
            for(size_t i = 0; i < data_.size(); i++)
            {
                if(data_[i].ground_truth_.rows() != 0)
                {
                    file << data_[i].image_->header.stamp << " ";
                    file << data_[i].ground_truth_.transpose() << endl;
                }
            }
            file.close();
        }
        else
        {
            cout << "could not open file " << path_ / ground_truth_filename_ << endl;
            exit(-1);
        }
    }

private:
    vector<DataFrame> data_;

    const boost::filesystem::path path_;
    const string image_topic_;
    const string info_topic_;
    const string measurements_filename_;
    const string ground_truth_filename_;
    const double admissible_delta_time_; // admissible time difference in s for comparing time stamps
};

#endif
<commit_msg>fixed some bugs<commit_after>/*************************************************************************
This software allows for filtering in high-dimensional measurement and
state spaces, as described in

M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal.
Probabilistic Object Tracking using a Range Camera
IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013

In a publication based on this software pleace cite the above reference.

Copyright (C) 2014  Manuel Wuthrich

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*************************************************************************/

#ifndef TRACKING_DATASET_HPP_
#define TRACKING_DATASET_HPP_

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

#include <fstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>

class DataFrame
{
public:
    sensor_msgs::Image::ConstPtr image_;
    sensor_msgs::CameraInfo::ConstPtr info_;
    Eigen::VectorXd ground_truth_;

    DataFrame(const sensor_msgs::Image::ConstPtr& image,
              const sensor_msgs::CameraInfo::ConstPtr& info,
              const Eigen::VectorXd& ground_truth = Eigen::VectorXd()):
        image_(image),
        info_(info),
        ground_truth_(ground_truth){ }
};

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
    void newMessage(const boost::shared_ptr<M const> &msg)
    {
        signalMessage(msg);
    }
};

class TrackingDataset
{
public:
    TrackingDataset(const std::string& path):path_(path),
                                image_topic_("XTION/depth/image"),
                                info_topic_("XTION/depth/camera_info"),
                                measurements_filename_("measurements.bag"),
                                ground_truth_filename_("ground_truth.txt"),
                                admissible_delta_time_(0.0001) {}
    ~TrackingDataset() {}

    void addFrame(const sensor_msgs::Image::ConstPtr& image,
                  const sensor_msgs::CameraInfo::ConstPtr& info,
                   const Eigen::VectorXd& ground_truth = Eigen::VectorXd())
    {
        DataFrame data(image, info, ground_truth);
        data_.push_back(data);
    }

    void addFrame(const sensor_msgs::Image::ConstPtr& image,
                  const sensor_msgs::CameraInfo::ConstPtr& info)
    {
        DataFrame data(image, info);
        data_.push_back(data);
    }

    sensor_msgs::Image::ConstPtr getImage(const size_t& index)
    {
        return data_[index].image_;
    }

    sensor_msgs::CameraInfo::ConstPtr getInfo(const size_t& index)
    {
        return data_[index].info_;
    }
    Eigen::Matrix3d getCameraMatrix(const size_t& index)
    {
        Eigen::Matrix3d camera_matrix;
        for(size_t col = 0; col < 3; col++)
            for(size_t row = 0; row < 3; row++)
                camera_matrix(row,col) = data_[0].info_->K[col+row*3];
        return camera_matrix;
    }

    Eigen::VectorXd getGroundTruth(const size_t& index)
    {
        return data_[index].ground_truth_;
    }

    size_t sIze()
    {
        return data_.size();
    }

    void loAd()
    {
        // load bagfile ----------------------------------------------------------------------------
        rosbag::Bag bag;
        bag.open((path_ / measurements_filename_).string(), rosbag::bagmode::Read);

        // Image topics to load
        std::vector<std::string> topics;
        topics.push_back(image_topic_);
        topics.push_back(info_topic_);
        rosbag::View view(bag, rosbag::TopicQuery(topics));

        // Set up fake subscribers to capture images
        BagSubscriber<sensor_msgs::Image> image_subscriber;
        BagSubscriber<sensor_msgs::CameraInfo> info_subscriber;

        // Use time synchronizer to make sure we get properly synchronized images
        message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo>
                sync(image_subscriber, info_subscriber, 25);
        sync.registerCallback(boost::bind(&TrackingDataset::addFrame, this,  _1, _2));

        // Load all messages into our stereo TrackingDataset
        BOOST_FOREACH(rosbag::MessageInstance const m, view)
        {
            if (m.getTopic() == image_topic_ || ("/" + m.getTopic() == image_topic_))
            {
                sensor_msgs::Image::ConstPtr image = m.instantiate<sensor_msgs::Image>();
                if (image != NULL)
                    image_subscriber.newMessage(image);
            }

            if (m.getTopic() == info_topic_ || ("/" + m.getTopic() == info_topic_))
            {
                sensor_msgs::CameraInfo::ConstPtr info = m.instantiate<sensor_msgs::CameraInfo>();
                if (info != NULL)
                    info_subscriber.newMessage(info);
            }
        }
        bag.close();

        // load ground_truth.txt ---------------------------------------------------------------------
        std::ifstream file; file.open((path_ / ground_truth_filename_).c_str(), std::ios::in); // open file
        if(file.is_open())
        {
            std::string temp; std::getline(file, temp); std::istringstream line(temp); // get a line from file
            double time_stamp; line >> time_stamp; // get the timestamp
            double scalar; Eigen::VectorXd state;
            while(line >> scalar) // get the state
            {
                Eigen::VectorXd temp(state.rows() + 1);
                temp.topRows(state.rows()) = state;
                temp(temp.rows()-1) = scalar;
                state = temp;
            }
            file.close();

            // attach the state to the appropriate data frames
            for(size_t i = 0; i < data_.size(); i++)
                if(std::fabs(data_[i].image_->header.stamp.toSec() - time_stamp) <= admissible_delta_time_)
                    data_[i].ground_truth_ = state;
        }
        else
        {
            std::cout << "could not open file " << path_ / ground_truth_filename_ << std::endl;
            exit(-1);
        }
    }

    void stOre()
    {
        if(boost::filesystem::exists(path_))
        {
//            std::cout << "TrackingDataset with name " << path_ << " already exists, will not overwrite." << std::endl;
//            return;
        }
        else
            boost::filesystem::create_directory(path_);

        // write images to bagfile -----------------------------------------------------------------
        rosbag::Bag bag;
        bag.open((path_ / measurements_filename_).string(), rosbag::bagmode::Write);

        std::vector<std::string> topics;
        topics.push_back(image_topic_);
        topics.push_back(info_topic_);

        for(size_t i = 0; i < data_.size(); i++)
        {
            bag.write(image_topic_, data_[i].image_->header.stamp, data_[i].image_);
            bag.write(info_topic_, data_[i].info_->header.stamp, data_[i].info_);
        }
        bag.close();

        // write ground truth to txt file ----------------------------------------------------------
        std::ofstream file; file.open((path_ / ground_truth_filename_).c_str(), std::ios::out | std::ios::trunc);
        if(file.is_open())
        {
            for(size_t i = 0; i < data_.size(); i++)
            {
                if(data_[i].ground_truth_.rows() != 0)
                {
                    file << data_[i].image_->header.stamp << " ";
                    file << data_[i].ground_truth_.transpose() << std::endl;
                }
            }
            file.close();
        }
        else
        {
            std::cout << "could not open file " << path_ / ground_truth_filename_ << std::endl;
            exit(-1);
        }
    }

private:
    std::vector<DataFrame> data_;

    const boost::filesystem::path path_;
    const std::string image_topic_;
    const std::string info_topic_;
    const std::string measurements_filename_;
    const std::string ground_truth_filename_;
    const double admissible_delta_time_; // admissible time difference in s for comparing time stamps
};

#endif
<|endoftext|>


<commit_before>
#define BOOST_FILESYSTEM_VERSION 2

// Headers
#include "OSGGLUT.h"
#include "OSGConfig.h"
#include "OSGSimpleGeometry.h"
#include "OSGGLUTWindow.h"
#include "OSGSimpleSceneManager.h"
#include "OSGAction.h"
#include "OSGSimpleGeometry.h"

// New Headers

// the general scene file loading handler
#include "OSGSceneFileHandler.h"
#include "OSGFieldContainerFactory.h"

#include <boost/bind.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

#include "OSGBackgroundLoader.h"
#include "OSGModelRequest.h"

// The SimpleSceneManager to manage simple applications
OSG::SimpleSceneManagerRefPtr mgr;
OSG::GroupNodeRefPtr          gScene;

unsigned gNextModelIdx = 0;

// forward declaration so we can have the interesting stuff upfront
int setupGLUT( int *argc, char *argv[] );

void findModels(std::string dirname)
{
   fs::path dir_path(dirname);

   if (!fs::exists(dir_path))
   {
      std::cerr << "ERROR: path does not exist: " << dirname << std::endl;
      gScene = static_cast<OSG::Node *>(NULL);
      OSG::osgExit();
      exit(-1);
   }

   fs::directory_iterator end_itr;
   for(fs::directory_iterator itr(dir_path); itr != end_itr; ++itr)
   {
      if(!fs::is_directory(*itr))
      {
         if (fs::extension(*itr) == std::string(".osb") ||
             fs::extension(*itr) == std::string(".wrl"))
         {
            fs::path  complete_file = fs::complete(*itr);
            std::string filename(complete_file.string());
            std::cout << "Found file: " << filename << std::endl;
            OSG::ModelRequestPtr req = OSG::ModelRequest::create()->init(OSG::NodeRefPtr(gScene.node()), filename);
            OSG::BackgroundLoader::the()->addRequest(req);
         }
      }
   }

}

// Initialize GLUT & OpenSG and set up the scene
int main(int argc, char **argv)
{
    // OSG init
    OSG::osgInit(argc,argv);

    gScene = OSG::GroupNodeRefPtr::create();

    if (argc < 2)
    {
       std::cout << "Specify a directory to load models from." << std::endl;
       gScene = static_cast<OSG::Node *>(NULL);
       OSG::osgExit();
       exit(-1);
    }

    findModels(std::string(argv[1]));

    // Start the background loader.
    OSG::BackgroundLoader::the()->start();

    // GLUT init
    int winid = setupGLUT(&argc, argv);

    // the connection between GLUT and OpenSG
    OSG::GLUTWindowUnrecPtr gwin= OSG::GLUTWindow::create();
    gwin->setGlutId(winid);
    gwin->init();

    // create the SimpleSceneManager helper
    mgr = OSG::SimpleSceneManager::create();

    // tell the manager what to manage
    mgr->setWindow(gwin );
    mgr->setRoot  (gScene.node());

    // show the whole scene
    mgr->showAll();

    // GLUT main loop
    glutMainLoop();

    OSG::BackgroundLoader::the()->stop();

   return 0;
}

//
// GLUT callback functions
//

// redraw the window
void display(void)
{
    OSG::Thread::getCurrentChangeList()->commitChangesAndClear();

    //OSG::BackgroundLoader::the()->processOne();
    // Sync changes from BackgroundLoader.
    OSG::BackgroundLoader::the()->sync();

    mgr->idle();
    mgr->redraw();
}

// react to size changes
void reshape(int w, int h)
{
    mgr->resize(w, h);
    glutPostRedisplay();
}

// react to mouse button presses
void mouse(int button, int state, int x, int y)
{

    if (state)
        mgr->mouseButtonRelease(button, x, y);
    else
        mgr->mouseButtonPress(button, x, y);

    glutPostRedisplay();
}

// react to mouse motions with pressed buttons
void motion(int x, int y)
{

    mgr->mouseMove(x, y);
    glutPostRedisplay();
}

// react to keys
void keyboard(unsigned char k, int , int )
{
    switch(k)
    {
        case 27:
        {
            mgr    = NULL;
            gScene = static_cast<OSG::Node *>(NULL);

            OSG::osgExit();
            exit(0);
        }
        break;

        case 'f':
        {
            mgr->setNavigationMode(OSG::Navigator::FLY);
        }
        break;

        case 't':
        {
            mgr->setNavigationMode(OSG::Navigator::TRACKBALL);
        }
        break;

    case 'a':
       {
          mgr->showAll();
       }
       break;

    case 's':
       {
          mgr->setStatistics(!mgr->getStatistics());
       }
       break;
    }
}

// setup the GLUT library which handles the windows for us
int setupGLUT(int *argc, char *argv[])
{
    glutInit(argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);

    int winid = glutCreateWindow("OpenSG");

    glutReshapeFunc(reshape);
    glutDisplayFunc(display);
    glutIdleFunc(display);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);

    return winid;
}
<commit_msg>fixed: make test work with boost filesystem v2/v3<commit_after>
// Headers
#include "OSGGLUT.h"
#include "OSGConfig.h"
#include "OSGSimpleGeometry.h"
#include "OSGGLUTWindow.h"
#include "OSGSimpleSceneManager.h"
#include "OSGAction.h"
#include "OSGSimpleGeometry.h"

// New Headers

// the general scene file loading handler
#include "OSGSceneFileHandler.h"
#include "OSGFieldContainerFactory.h"

#include <boost/bind.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

#include "OSGBackgroundLoader.h"
#include "OSGModelRequest.h"

// The SimpleSceneManager to manage simple applications
OSG::SimpleSceneManagerRefPtr mgr;
OSG::GroupNodeRefPtr          gScene;

unsigned gNextModelIdx = 0;

// forward declaration so we can have the interesting stuff upfront
int setupGLUT( int *argc, char *argv[] );

void findModels(std::string dirname)
{
   fs::path dir_path(dirname);

   if (!fs::exists(dir_path))
   {
      std::cerr << "ERROR: path does not exist: " << dirname << std::endl;
      gScene = static_cast<OSG::Node *>(NULL);
      OSG::osgExit();
      exit(-1);
   }

   fs::directory_iterator end_itr;
   for(fs::directory_iterator itr(dir_path); itr != end_itr; ++itr)
   {
      if(!fs::is_directory(*itr))
      {
         if (fs::extension(*itr) == std::string(".osb") ||
             fs::extension(*itr) == std::string(".wrl"))
         {
#if BOOST_FILESYSTEM_VERSION == 3
            fs::path complete_file = fs::absolute(*itr);
#else
            fs::path complete_file = fs::complete(*itr);
#endif
            std::string filename(complete_file.string());
            std::cout << "Found file: " << filename << std::endl;
            OSG::ModelRequestPtr req = OSG::ModelRequest::create()->init(OSG::NodeRefPtr(gScene.node()), filename);
            OSG::BackgroundLoader::the()->addRequest(req);
         }
      }
   }

}

// Initialize GLUT & OpenSG and set up the scene
int main(int argc, char **argv)
{
    // OSG init
    OSG::osgInit(argc,argv);

    gScene = OSG::GroupNodeRefPtr::create();

    if (argc < 2)
    {
       std::cout << "Specify a directory to load models from." << std::endl;
       gScene = static_cast<OSG::Node *>(NULL);
       OSG::osgExit();
       exit(-1);
    }

    findModels(std::string(argv[1]));

    // Start the background loader.
    OSG::BackgroundLoader::the()->start();

    // GLUT init
    int winid = setupGLUT(&argc, argv);

    // the connection between GLUT and OpenSG
    OSG::GLUTWindowUnrecPtr gwin= OSG::GLUTWindow::create();
    gwin->setGlutId(winid);
    gwin->init();

    // create the SimpleSceneManager helper
    mgr = OSG::SimpleSceneManager::create();

    // tell the manager what to manage
    mgr->setWindow(gwin );
    mgr->setRoot  (gScene.node());

    // show the whole scene
    mgr->showAll();

    // GLUT main loop
    glutMainLoop();

    OSG::BackgroundLoader::the()->stop();

   return 0;
}

//
// GLUT callback functions
//

// redraw the window
void display(void)
{
    OSG::Thread::getCurrentChangeList()->commitChangesAndClear();

    //OSG::BackgroundLoader::the()->processOne();
    // Sync changes from BackgroundLoader.
    OSG::BackgroundLoader::the()->sync();

    mgr->idle();
    mgr->redraw();
}

// react to size changes
void reshape(int w, int h)
{
    mgr->resize(w, h);
    glutPostRedisplay();
}

// react to mouse button presses
void mouse(int button, int state, int x, int y)
{

    if (state)
        mgr->mouseButtonRelease(button, x, y);
    else
        mgr->mouseButtonPress(button, x, y);

    glutPostRedisplay();
}

// react to mouse motions with pressed buttons
void motion(int x, int y)
{

    mgr->mouseMove(x, y);
    glutPostRedisplay();
}

// react to keys
void keyboard(unsigned char k, int , int )
{
    switch(k)
    {
        case 27:
        {
            mgr    = NULL;
            gScene = static_cast<OSG::Node *>(NULL);

            OSG::osgExit();
            exit(0);
        }
        break;

        case 'f':
        {
            mgr->setNavigationMode(OSG::Navigator::FLY);
        }
        break;

        case 't':
        {
            mgr->setNavigationMode(OSG::Navigator::TRACKBALL);
        }
        break;

    case 'a':
       {
          mgr->showAll();
       }
       break;

    case 's':
       {
          mgr->setStatistics(!mgr->getStatistics());
       }
       break;
    }
}

// setup the GLUT library which handles the windows for us
int setupGLUT(int *argc, char *argv[])
{
    glutInit(argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);

    int winid = glutCreateWindow("OpenSG");

    glutReshapeFunc(reshape);
    glutDisplayFunc(display);
    glutIdleFunc(display);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);

    return winid;
}
<|endoftext|>