text:
/*
 * GridMapCvProcessing.cpp
 *
 *  Created on: Apr 15, 2016
 *      Author: Péter Fankhauser, Magnus Gärtner
 *	 Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_cv/GridMapCvProcessing.hpp"
#include "grid_map_cv/GridMapCvConverter.hpp"

#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>

namespace grid_map {

GridMapCvProcessing::GridMapCvProcessing() {}

GridMapCvProcessing::~GridMapCvProcessing() {}

bool GridMapCvProcessing::changeResolution(const GridMap& gridMapSource, GridMap& gridMapResult, const double resolution,
                                           const int interpolationAlgorithm) {
  GridMap gridMapSourceCopy(gridMapSource);
  gridMapSourceCopy.convertToDefaultStartIndex();
  const double sizeFactor = gridMapSourceCopy.getResolution() / resolution;
  bool firstLayer = true;
  for (const auto& layer : gridMapSourceCopy.getLayers()) {
    cv::Mat imageSource, imageResult;
    const float minValue = gridMapSourceCopy.get(layer).minCoeffOfFinites();
    const float maxValue = gridMapSourceCopy.get(layer).maxCoeffOfFinites();
    const bool hasNaN = gridMapSourceCopy.get(layer).hasNaN();
    bool result;
    if (hasNaN) {
      result = GridMapCvConverter::toImage<unsigned short, 4>(gridMapSourceCopy, layer, CV_16UC4, minValue, maxValue, imageSource);
    } else {
      result = GridMapCvConverter::toImage<unsigned short, 1>(gridMapSourceCopy, layer, CV_16UC1, minValue, maxValue, imageSource);
    }
    if (!result) return false;
    cv::resize(imageSource, imageResult, cv::Size(0.0, 0.0), sizeFactor, sizeFactor, interpolationAlgorithm);
    if (firstLayer) {
      if (!GridMapCvConverter::initializeFromImage(imageResult, resolution, gridMapResult, gridMapSourceCopy.getPosition())) return false;
      firstLayer = false;
    }
    if (hasNaN) {
      result = GridMapCvConverter::addLayerFromImage<unsigned short, 4>(imageResult, layer, gridMapResult, minValue, maxValue);
    } else {
      result = GridMapCvConverter::addLayerFromImage<unsigned short, 1>(imageResult, layer, gridMapResult, minValue, maxValue);
    }
    if (!result) return false;
  }
  gridMapResult.setFrameId(gridMapSourceCopy.getFrameId());
  gridMapResult.setTimestamp(gridMapSourceCopy.getTimestamp());
  gridMapResult.setBasicLayers(gridMapSourceCopy.getBasicLayers());
  return true;
}

GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Eigen::Isometry3d& transform,
                                               const std::string& heightLayerName, const std::string& newFrameId) {
  // Check if height layer is valid.
  if (!gridMapSource.exists(heightLayerName)) {
    throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available.");
  }

  // Check if transformation is z aligned.
  if (std::abs(transform(2, 2) - 1) >= 0.05) {
    throw std::invalid_argument("The given transform is not Z aligned!");
  }

  auto yawPitchRoll = transform.rotation().eulerAngles(2, 1, 0);  // Double check convention!
  double rotationAngle = yawPitchRoll.x() * 180 / CV_PI;
  if (std::abs(yawPitchRoll.y()) >= 3 && std::abs(yawPitchRoll.z()) >= 3) {  // Resolve yaw ambiguity in euler angles.
    rotationAngle += 180;
  }

  gridMapSource.convertToDefaultStartIndex();

  // Create the rotated gridMap.
  GridMap transformedMap(gridMapSource.getLayers());
  transformedMap.setBasicLayers(gridMapSource.getBasicLayers());
  transformedMap.setTimestamp(gridMapSource.getTimestamp());
  transformedMap.setFrameId(newFrameId);

  // openCV rotation parameters, initalized on first layer.
  cv::Mat imageRotationMatrix;
  cv::Rect2f boundingBox;

  bool firstLayer = true;
  for (const auto& layer : gridMapSource.getLayers()) {
    cv::Mat imageSource, imageResult;

    // From gridMap to openCV image. Assumes defaultStartIndex.
    cv::eigen2cv(gridMapSource[layer], imageSource);

    // Calculate transformation matrix and update geometry of the resulting grid map.
    if (firstLayer) {
      // Get rotation matrix for rotating the image around its center in pixel coordinates. See
      // https://answers.opencv.org/question/82708/rotation-center-confusion/
      cv::Point2f imageCenter =
          cv::Point2f((imageSource.cols - 1) / 2.0, (imageSource.rows - 1) / 2.0);

      imageRotationMatrix = cv::getRotationMatrix2D(imageCenter, rotationAngle, 1.0);
      boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();

      // Adjust transformation matrix. See
      // https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#gafbbc470ce83812914a70abfb604f4326 and https://stackoverflow.com/questions/22041699/rotate-an-image-without-cropping-in-opencv-in-c
      imageRotationMatrix.at<double>(0, 2) += boundingBox.width / 2.0 - imageSource.cols / 2.0;
      imageRotationMatrix.at<double>(1, 2) += boundingBox.height / 2.0 - imageSource.rows / 2.0;

      // Calculate the new center of the gridMap.
      Position3 newCenter = transform * Position3{(gridMapSource.getPosition().x()), (gridMapSource.getPosition().y()), 0.0};

      // Set the size of the rotated gridMap.
      transformedMap.setGeometry({boundingBox.height * gridMapSource.getResolution(), boundingBox.width * gridMapSource.getResolution()},
                         gridMapSource.getResolution(), Position(newCenter.x(), newCenter.y()));
      firstLayer = false;
    }

    // Rotate the layer.
    imageResult = cv::Mat(boundingBox.size(), CV_32F, std::numeric_limits<double>::quiet_NaN());
    cv::warpAffine(imageSource, imageResult, imageRotationMatrix, boundingBox.size(), cv::INTER_NEAREST, cv::BORDER_TRANSPARENT);

    // Copy result into gridMapLayer. Assumes default start index.
    Matrix resultLayer;
    cv::cv2eigen(imageResult, resultLayer);
    transformedMap.add(layer, resultLayer);
  }

  // Add height translation.
  grid_map::Matrix heightLayer = transformedMap[heightLayerName];
  transformedMap[heightLayerName] = heightLayer.array() + transform.translation().z();

  return transformedMap;
}
}  // namespace grid_map


text:
#ifndef BOOST_DETAIL_SP_TYPEINFO_HPP_INCLUDED
#define BOOST_DETAIL_SP_TYPEINFO_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

//  detail/sp_typeinfo.hpp
//
//  Copyright 2007 Peter Dimov
//
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>

#if defined( BOOST_NO_TYPEID )

#include <boost/current_function.hpp>
#include <functional>

namespace boost
{

namespace detail
{

class sp_typeinfo
{
private:

    sp_typeinfo( sp_typeinfo const& );
    sp_typeinfo& operator=( sp_typeinfo const& );

    char const * name_;

public:

    explicit sp_typeinfo( char const * name ): name_( name )
    {
    }

    bool operator==( sp_typeinfo const& rhs ) const
    {
        return this == &rhs;
    }

    bool operator!=( sp_typeinfo const& rhs ) const
    {
        return this != &rhs;
    }

    bool before( sp_typeinfo const& rhs ) const
    {
        return std::less< sp_typeinfo const* >()( this, &rhs );
    }

    char const* name() const
    {
        return name_;
    }
};

template<class T> struct sp_typeid_
{
    static sp_typeinfo ti_;

    static char const * name()
    {
        return BOOST_CURRENT_FUNCTION;
    }
};

template<class T> sp_typeinfo sp_typeid_< T >::ti_ = sp_typeid_< T >::name();

template<class T> struct sp_typeid_< T & >: sp_typeid_< T >
{
};

template<class T> struct sp_typeid_< T const >: sp_typeid_< T >
{
};

template<class T> struct sp_typeid_< T volatile >: sp_typeid_< T >
{
};

template<class T> struct sp_typeid_< T const volatile >: sp_typeid_< T >
{
};

} // namespace detail

} // namespace boost

#define BOOST_SP_TYPEID(T) (boost::detail::sp_typeid_<T>::ti_)

#else

#include <typeinfo>

namespace boost
{

namespace detail
{

#if defined( BOOST_NO_STD_TYPEINFO )

typedef ::type_info sp_typeinfo;

#else

typedef std::type_info sp_typeinfo;

#endif

} // namespace detail

} // namespace boost

#define BOOST_SP_TYPEID(T) typeid(T)

#endif

#endif  // #ifndef BOOST_DETAIL_SP_TYPEINFO_HPP_INCLUDED

ext: hpp

max_stars_repo_name: MIPT-ILab/mdsp

max_issues_repo_name: MIPT-ILab/mdsp

max_forks_repo_name: MIPT-ILab/mdsp


text:
/*=============================================================================
    Copyright (c) 2003 Joel de Guzman

    Use, modification and distribution is subject to the Boost Software
    License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#if !defined(FUSION_SEQUENCE_FILTER_VIEW_HPP)
#define FUSION_SEQUENCE_FILTER_VIEW_HPP

#include <boost/spirit/fusion/detail/access.hpp>
#include <boost/spirit/fusion/iterator/filter_view_iterator.hpp>
#include <boost/spirit/fusion/sequence/detail/filter_view_begin_end_trts.hpp>
#include <boost/spirit/fusion/sequence/detail/sequence_base.hpp>
#include <boost/spirit/fusion/sequence/as_fusion_sequence.hpp>
#include <boost/spirit/fusion/sequence/begin.hpp>
#include <boost/spirit/fusion/sequence/end.hpp>

namespace boost { namespace fusion
{
    struct filter_view_tag;

    template <typename Sequence, typename Pred>
    struct filter_view : sequence_base<filter_view<Sequence, Pred> >
    {
        typedef as_fusion_sequence<Sequence> seq_converter;
        typedef typename seq_converter::type seq;

        typedef filter_view_tag tag;
        typedef typename meta::begin<seq>::type first_type;
        typedef typename meta::end<seq>::type last_type;
        typedef Pred pred_type;

        filter_view(Sequence& seq)
            : first(fusion::begin(seq))
            , last(fusion::end(seq))
        {}

        first_type first;
        last_type last;
    };
}}

#endif

max_stars_repo_name: dstrigl/mcotf

max_issues_repo_name: dstrigl/mcotf

max_forks_repo_name: dstrigl/mcotf


text: #include <boost/serialization/array_wrapper.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
//  (C) Copyright Gennadiy Rozental 2001.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : implements compiler like Log formatter
// ***************************************************************************

#ifndef BOOST_TEST_COMPILER_LOG_FORMATTER_IPP_020105GER
#define BOOST_TEST_COMPILER_LOG_FORMATTER_IPP_020105GER

// Boost.Test
#include <boost/test/output/compiler_log_formatter.hpp>

#include <boost/test/framework.hpp>
#include <boost/test/execution_monitor.hpp>
#include <boost/test/unit_test_parameters.hpp>

#include <boost/test/tree/test_unit.hpp>

#include <boost/test/utils/basic_cstring/io.hpp>
#include <boost/test/utils/lazy_ostream.hpp>
#include <boost/test/utils/setcolor.hpp>

// Boost
#include <boost/version.hpp>

// STL
#include <iostream>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace output {

// ************************************************************************** //
// **************            compiler_log_formatter            ************** //
// ************************************************************************** //

namespace {

std::string
test_phase_identifier()
{
    return framework::test_in_progress() ? framework::current_test_unit().full_name() : std::string( "Test setup" );
}

} // local namespace

//____________________________________________________________________________//

void
compiler_log_formatter::log_start( std::ostream& output, counter_t test_cases_amount )
{
    m_color_output = runtime_config::get<bool>( runtime_config::btrt_color_output );

    if( test_cases_amount > 0 )
        output  << "Running " << test_cases_amount << " test "
                << (test_cases_amount > 1 ? "cases" : "case") << "...\n";
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_finish( std::ostream& ostr )
{
    ostr.flush();
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_build_info( std::ostream& output )
{
    output  << "Platform: " << BOOST_PLATFORM            << '\n'
            << "Compiler: " << BOOST_COMPILER            << '\n'
            << "STL     : " << BOOST_STDLIB              << '\n'
            << "Boost   : " << BOOST_VERSION/100000      << "."
                            << BOOST_VERSION/100 % 1000  << "."
                            << BOOST_VERSION % 100       << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::test_unit_start( std::ostream& output, test_unit const& tu )
{
    BOOST_TEST_SCOPE_SETCOLOR( m_color_output, output, term_attr::BRIGHT, term_color::BLUE );

    print_prefix( output, tu.p_file_name, tu.p_line_num );

    output << "Entering test " << tu.p_type_name << " \"" << tu.p_name << "\"" << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::test_unit_finish( std::ostream& output, test_unit const& tu, unsigned long elapsed )
{
    BOOST_TEST_SCOPE_SETCOLOR( m_color_output, output, term_attr::BRIGHT, term_color::BLUE );

    print_prefix( output, tu.p_file_name, tu.p_line_num );

    output << "Leaving test " << tu.p_type_name << " \"" << tu.p_name << "\"";

    if( elapsed > 0 ) {
        output << "; testing time: ";
        if( elapsed % 1000 == 0 )
            output << elapsed/1000 << "ms";
        else
            output << elapsed << "us";
    }

    output << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::test_unit_skipped( std::ostream& output, test_unit const& tu, const_string reason )
{
    BOOST_TEST_SCOPE_SETCOLOR( m_color_output, output, term_attr::BRIGHT, term_color::YELLOW );

    print_prefix( output, tu.p_file_name, tu.p_line_num );

    output  << "Test " << tu.p_type_name << " \"" << tu.full_name() << "\"" << " is skipped because " << reason << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_exception_start( std::ostream& output, log_checkpoint_data const& checkpoint_data, execution_exception const& ex )
{
    execution_exception::location const& loc = ex.where();

    print_prefix( output, loc.m_file_name, loc.m_line_num );

    {
        BOOST_TEST_SCOPE_SETCOLOR( m_color_output, output, term_attr::UNDERLINE, term_color::RED );

        output << "fatal error: in \"" << (loc.m_function.is_empty() ? test_phase_identifier() : loc.m_function ) << "\": "
               << ex.what();
    }

    if( !checkpoint_data.m_file_name.is_empty() ) {
        output << '\n';
        print_prefix( output, checkpoint_data.m_file_name, checkpoint_data.m_line_num );

        BOOST_TEST_SCOPE_SETCOLOR( m_color_output, output, term_attr::BRIGHT, term_color::CYAN );

        output << "last checkpoint";
        if( !checkpoint_data.m_message.empty() )
            output << ": " << checkpoint_data.m_message;
    }
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_exception_finish( std::ostream& output )
{
    output << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_entry_start( std::ostream& output, log_entry_data const& entry_data, log_entry_types let )
{
    using namespace utils;

    switch( let ) {
        case BOOST_UTL_ET_INFO:
            print_prefix( output, entry_data.m_file_name, entry_data.m_line_num );
            if( m_color_output )
                output << setcolor( term_attr::BRIGHT, term_color::GREEN );
            output << "info: ";
            break;
        case BOOST_UTL_ET_MESSAGE:
            if( m_color_output )
                output << setcolor( term_attr::BRIGHT, term_color::CYAN );
            break;
        case BOOST_UTL_ET_WARNING:
            print_prefix( output, entry_data.m_file_name, entry_data.m_line_num );
            if( m_color_output )
                output << setcolor( term_attr::BRIGHT, term_color::YELLOW );
            output << "warning: in \"" << test_phase_identifier() << "\": ";
            break;
        case BOOST_UTL_ET_ERROR:
            print_prefix( output, entry_data.m_file_name, entry_data.m_line_num );
            if( m_color_output )
                output << setcolor( term_attr::BRIGHT, term_color::RED );
            output << "error: in \"" << test_phase_identifier() << "\": ";
            break;
        case BOOST_UTL_ET_FATAL_ERROR:
            print_prefix( output, entry_data.m_file_name, entry_data.m_line_num );
            if( m_color_output )
                output << setcolor( term_attr::UNDERLINE, term_color::RED );
            output << "fatal error: in \"" << test_phase_identifier() << "\": ";
            break;
    }
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_entry_value( std::ostream& output, const_string value )
{
    output << value;
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_entry_value( std::ostream& output, lazy_ostream const& value )
{
    output << value;
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_entry_finish( std::ostream& output )
{
    if( m_color_output )
        output << utils::setcolor();

    output << std::endl;
}

//____________________________________________________________________________//

void
compiler_log_formatter::print_prefix( std::ostream& output, const_string file_name, std::size_t line_num )
{
    if( !file_name.empty() ) {
#ifdef __APPLE_CC__
        // Xcode-compatible logging format, idea by Richard Dingwall at
        // <http://richarddingwall.name/2008/06/01/using-the-boost-unit-test-framework-with-xcode-3/>.
        output << file_name << ':' << line_num << ": ";
#else
        output << file_name << '(' << line_num << "): ";
#endif
    }
}

//____________________________________________________________________________//

void
compiler_log_formatter::entry_context_start( std::ostream& output, log_level l )
{
    if( l == log_messages ) {
        output << "\n[context:";
    }
    else {
        output << (l == log_successful_tests ? "\nAssertion" : "\nFailure" ) << " occurred in a following context:";
    }
}

//____________________________________________________________________________//

void
compiler_log_formatter::entry_context_finish( std::ostream& output, log_level l )
{
    if( l == log_messages ) {
        output << "]";
    }
    output.flush();
}

//____________________________________________________________________________//

void
compiler_log_formatter::log_entry_context( std::ostream& output, log_level l, const_string context_descr )
{
    output << "\n    " << context_descr;
}

//____________________________________________________________________________//

} // namespace output
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_COMPILER_LOG_FORMATTER_IPP_020105GER

ext: ipp