<commit_before>/*---------------------------------------------------------------------------*\
*                                OpenSG                                     *
*                                                                           *
*                                                                           *
*             Copyright (C) 2000-2002 by the OpenSG Forum                   *
*                                                                           *
*                            www.opensg.org                                 *
*                                                                           *
*   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
*                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
*                                License                                    *
*                                                                           *
* This library is free software; you can redistribute it and/or modify it   *
* under the terms of the GNU Library General Public License as published    *
* by the Free Software Foundation, version 2.                               *
*                                                                           *
* This library 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         *
* Library General Public License for more details.                          *
*                                                                           *
* You should have received a copy of the GNU Library General Public         *
* License along with this library; if not, write to the Free Software       *
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
*                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
*                                Changes                                    *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
\*---------------------------------------------------------------------------*/

//-------------------------------
//  Includes
//-------------------------------

#include <stdlib.h>
#include <stdio.h>

#include "OSGConfig.h"

#include <iostream>
#include <fstream>

#include <OSGLog.h>

#include <OSGNode.h>
#include <OSGGeometry.h>
#include <OSGSimpleMaterial.h>
#include <OSGTypedGeoIntegralProperty.h>
#include <OSGSimpleAttachments.h>
//#include <OSGSceneFileHandler.h>

#include "OSGSTLSceneFileType.h"

OSG_USING_NAMESPACE

#if defined(OSG_WIN32_ICL) && !defined(OSG_CHECK_FIELDSETARG)
#pragma warning (disable : 383)
#endif

/*! \class OSG::STLSceneFileType
    \ingroup GrpSystemFileIO

*/

/*****************************
*   Types
*****************************/
// Static Class Varible implementations:

const Char8            *STLSceneFileType::_suffixA[] = {"stl", "stla", "stlb"};

STLSceneFileType  STLSceneFileType::_the(_suffixA,
                                        sizeof(_suffixA),
                                        false,
                                        10,
                                        SceneFileType::OSG_READ_SUPPORTED);

/*****************************
*    Classvariables
*****************************/

/********************************
*    Class methodes
*******************************/

/*******************************
*public
*******************************/

//----------------------------
// Function name: read
//----------------------------
//
//Parameters:
//p: Scene &image, const char *fileName
//GlobalVars:
//g:
//Returns:
//r:bool
// Caution
//c:
//Assumations:
//a:
//Describtions:
//d: read the image from the given file
//SeeAlso:
//s:
//
//------------------------------

#ifdef __sgi
#pragma set woff 1209
#endif

NodeTransitPtr STLSceneFileType::read(      std::istream &is,
                                    const Char8        *fileNameOrExtension,
                                            Resolver        ) const
{

    NodeTransitPtr              root;
    GeometryUnrecPtr            geo;
    GeoPnt3rPropertyUnrecPtr    points;
    GeoVec3rPropertyUnrecPtr    normals;
    GeoIntegralPropertyUnrecPtr index;
    GeoIntegralPropertyUnrecPtr lens;
    GeoIntegralPropertyUnrecPtr type;

    STLFaceList theFaces;
    std::string theName;
    bool        isAscii = isASCII(is, fileNameOrExtension);

    if(isAscii)
    {
        if(!readASCII(is, theFaces, theName))
            return root;
    }
    else
    {
        if(!readBinary(is, theFaces, theName))
            return root;
    }
    if(!theFaces.empty())
    {
        geo = Geometry::create();

        points = GeoPnt3rProperty::create();
        geo->setPositions(points);
        normals = GeoVec3rProperty::create();
        geo->setNormals(normals);
        lens = GeoUIntProperty::create();
        geo->setLengths(lens);
        lens->push_back(theFaces.size()*3);
        type = GeoUInt8Property::create();
        geo->setTypes(type);
        type->push_back(GL_TRIANGLES);

        root = Node::create();
        //setName(root, theName.c_str());

        GeoPnt3rProperty::StoredFieldType *fPos  = points ->editFieldPtr();
        GeoVec3rProperty::StoredFieldType *fNorm = normals->editFieldPtr();

        STLFaceListIterator actIt, endIt;
        actIt = theFaces.begin();
        endIt = theFaces.end();

        while(actIt != endIt)
        {
            fPos ->push_back(Pnt3f(actIt->v1x, actIt->v1y, actIt->v1z));
            fPos ->push_back(Pnt3f(actIt->v2x, actIt->v2y, actIt->v2z));
            fPos ->push_back(Pnt3f(actIt->v3x, actIt->v3y, actIt->v3z));

            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));
            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));
            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));

            ++actIt;
        }

        geo->setTypes    (type);
        geo->setLengths  (lens);
        geo->setPositions(points);
        geo->setNormals  (normals);

        geo->setMaterial(getDefaultMaterial());

        root->setCore(geo);
    }

    return root;

}

bool STLSceneFileType::isASCII(std::istream &is, const Char8* fileNameOrExtension) const
{
    std::string theAbsName = fileNameOrExtension;
    std::string theExtension;
    int nameLength = theAbsName.length();
    int separator  = theAbsName.rfind(".", nameLength);
    theExtension = theAbsName.substr(separator+1, nameLength-separator-1);

    for (int i = 0; i < theExtension.length(); i++)
        theExtension[i] = tolower(theExtension[i]);

    if (theExtension.compare("stla") == 0)
        return true;
    else if (theExtension.compare("stlb") == 0)
        return false;
    else
    {
        // we have to check if our file start with "solid"
        std::string theCheck;
        is >> theCheck;
        for (int i = 0; i< theCheck.length(); i++)
                theCheck[i] = tolower(theCheck[i]);

        is.seekg( 0, std::ios_base::beg );

        return (theCheck.compare("solid") == 0);
    }
}

bool STLSceneFileType::readASCII(std::istream &is, STLFaceList& theFaces, std::string& theName) const
{
    FDEBUG(("STLSceneFileType::readASCII:\n"));

    char token[9];
    int finished = 0;
    int parseResult = 0;
    UInt32 i;

    if(!is)
    return false;

    while ((finished == 0) && (! (is.eof())))
    {
        //SceneFileHandler::the().updateReadProgress();
        memset(token, ' ', 9);
        is >> token;
        for (i = 0; i < strlen(token); i++)
        {
            token[i] = tolower(token[i]);
        }
        if (strcmp(token, "solid") == 0)
        {
            char tmp = is.get();
            if (tmp == ' ')
            is >> theName;
        }
        else if (strcmp(token, "facet") == 0)
        {
            STLFace tmpFace;
            is >> token >> tmpFace.nx >> tmpFace.ny >> tmpFace.nz;
            is >> token >> token;// skip "outer loop"
            is >> token >> tmpFace.v1x >> tmpFace.v1y >> tmpFace.v1z;
            is >> token >> tmpFace.v2x >> tmpFace.v2y >> tmpFace.v2z;
            is >> token >> tmpFace.v3x >> tmpFace.v3y >> tmpFace.v3z;
            is >> token;// skip "endloop"
            is >> token;// skip "endfacet"
            theFaces.push_back(tmpFace);
        }
        else if (strcmp(token, "endsolid") == 0)
        {
            finished = 1;
            return true;
        }
    }
    return false;
}

bool STLSceneFileType::readBinary(std::istream &is, STLFaceList& theFaces, std::string& theName) const
{
    FDEBUG(("STLSceneFileType::readBinary:\n"));

    if(!is)
        return false;

    bool bigEndian = osgIsBigEndian();

    int HeaderSize = 84;    // 80bytes for name and 4 bytes for number of facets
    char name[80] = "";
    is.read( &name[0], 80 );
    theName = name;
    is.read(&name[0], 4); // skip the number of facets
    while (!is.eof())
    {
        //SceneFileHandler::the().updateReadProgress();
        // a binary facet block has 50 bytes
        STLFace tmpFace;

        tmpFace.nx = readFloat(is, bigEndian);
        tmpFace.ny = readFloat(is, bigEndian);
        tmpFace.nz = readFloat(is, bigEndian);

        tmpFace.v1x = readFloat(is, bigEndian);
        tmpFace.v1y = readFloat(is, bigEndian);
        tmpFace.v1z = readFloat(is, bigEndian);

        tmpFace.v2x = readFloat(is, bigEndian);
        tmpFace.v2y = readFloat(is, bigEndian);
        tmpFace.v2z = readFloat(is, bigEndian);

        tmpFace.v3x = readFloat(is, bigEndian);
        tmpFace.v3y = readFloat(is, bigEndian);
        tmpFace.v3z = readFloat(is, bigEndian);

        theFaces.push_back(tmpFace);
        char buffer[2]; // offset filler
        is.read (&buffer[0], 2);
    }
    return true;
}

Real32 STLSceneFileType::readFloat(std::istream& is, bool bigEndian) const
{
    Real32 result = 0;

    unsigned char buffer[4];
    is.read (reinterpret_cast<char*>(&buffer[0]), 4);
    Real32* tmp = reinterpret_cast<Real32*>(&buffer);
    result = *tmp;
    if (bigEndian)
        result = osgNetToHost(result);
    return result;
}

#ifdef __sgi
#pragma reset woff 1209
#endif

/******************************
*protected
******************************/

/******************************
*private
******************************/

/***************************
*instance methodes
***************************/

/***************************
*public
***************************/

/**constructors & destructors**/

STLSceneFileType::STLSceneFileType(const Char8  *suffixArray[],
                                        UInt16  suffixByteCount,
                                        bool    override,
                                        UInt32  overridePriority,
                                        UInt32  flags) :
    SceneFileType(suffixArray,
                suffixByteCount,
                override,
                overridePriority,
                flags)
{
}

STLSceneFileType &STLSceneFileType::the(void)
{
    return _the;
}

const Char8 *STLSceneFileType::getName(void) const
{
    return "Stereolithography Geometry";
}

STLSceneFileType::~STLSceneFileType (void )
{
    return;
}

<commit_msg>fixed: removed deprecated includes<commit_after>/*---------------------------------------------------------------------------*\
*                                OpenSG                                     *
*                                                                           *
*                                                                           *
*             Copyright (C) 2000-2002 by the OpenSG Forum                   *
*                                                                           *
*                            www.opensg.org                                 *
*                                                                           *
*   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
*                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
*                                License                                    *
*                                                                           *
* This library is free software; you can redistribute it and/or modify it   *
* under the terms of the GNU Library General Public License as published    *
* by the Free Software Foundation, version 2.                               *
*                                                                           *
* This library 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         *
* Library General Public License for more details.                          *
*                                                                           *
* You should have received a copy of the GNU Library General Public         *
* License along with this library; if not, write to the Free Software       *
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
*                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
*                                Changes                                    *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
*                                                                           *
\*---------------------------------------------------------------------------*/

//-------------------------------
//  Includes
//-------------------------------

#include <stdlib.h>
#include <stdio.h>

#include "OSGConfig.h"

#include <iostream>
#include <fstream>

#include <OSGLog.h>

#include <OSGNode.h>
#include <OSGGeometry.h>
#include <OSGSimpleMaterial.h>
#include <OSGTypedGeoIntegralProperty.h>

#include "OSGSTLSceneFileType.h"

OSG_USING_NAMESPACE

#if defined(OSG_WIN32_ICL) && !defined(OSG_CHECK_FIELDSETARG)
#pragma warning (disable : 383)
#endif

/*! \class OSG::STLSceneFileType
    \ingroup GrpSystemFileIO

*/

/*****************************
*   Types
*****************************/
// Static Class Varible implementations:

const Char8            *STLSceneFileType::_suffixA[] = {"stl", "stla", "stlb"};

STLSceneFileType  STLSceneFileType::_the(_suffixA,
                                        sizeof(_suffixA),
                                        false,
                                        10,
                                        SceneFileType::OSG_READ_SUPPORTED);

/*****************************
*    Classvariables
*****************************/

/********************************
*    Class methodes
*******************************/

/*******************************
*public
*******************************/

//----------------------------
// Function name: read
//----------------------------
//
//Parameters:
//p: Scene &image, const char *fileName
//GlobalVars:
//g:
//Returns:
//r:bool
// Caution
//c:
//Assumations:
//a:
//Describtions:
//d: read the image from the given file
//SeeAlso:
//s:
//
//------------------------------

#ifdef __sgi
#pragma set woff 1209
#endif

NodeTransitPtr STLSceneFileType::read(      std::istream &is,
                                    const Char8        *fileNameOrExtension,
                                            Resolver        ) const
{

    NodeTransitPtr              root;
    GeometryUnrecPtr            geo;
    GeoPnt3rPropertyUnrecPtr    points;
    GeoVec3rPropertyUnrecPtr    normals;
    GeoIntegralPropertyUnrecPtr index;
    GeoIntegralPropertyUnrecPtr lens;
    GeoIntegralPropertyUnrecPtr type;

    STLFaceList theFaces;
    std::string theName;
    bool        isAscii = isASCII(is, fileNameOrExtension);

    if(isAscii)
    {
        if(!readASCII(is, theFaces, theName))
            return root;
    }
    else
    {
        if(!readBinary(is, theFaces, theName))
            return root;
    }
    if(!theFaces.empty())
    {
        geo = Geometry::create();

        points = GeoPnt3rProperty::create();
        geo->setPositions(points);
        normals = GeoVec3rProperty::create();
        geo->setNormals(normals);
        lens = GeoUIntProperty::create();
        geo->setLengths(lens);
        lens->push_back(theFaces.size()*3);
        type = GeoUInt8Property::create();
        geo->setTypes(type);
        type->push_back(GL_TRIANGLES);

        root = Node::create();
        //setName(root, theName.c_str());

        GeoPnt3rProperty::StoredFieldType *fPos  = points ->editFieldPtr();
        GeoVec3rProperty::StoredFieldType *fNorm = normals->editFieldPtr();

        STLFaceListIterator actIt, endIt;
        actIt = theFaces.begin();
        endIt = theFaces.end();

        while(actIt != endIt)
        {
            fPos ->push_back(Pnt3f(actIt->v1x, actIt->v1y, actIt->v1z));
            fPos ->push_back(Pnt3f(actIt->v2x, actIt->v2y, actIt->v2z));
            fPos ->push_back(Pnt3f(actIt->v3x, actIt->v3y, actIt->v3z));

            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));
            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));
            fNorm->push_back(Vec3f(actIt->nx,  actIt->ny,  actIt->nz));

            ++actIt;
        }

        geo->setTypes    (type);
        geo->setLengths  (lens);
        geo->setPositions(points);
        geo->setNormals  (normals);

        geo->setMaterial(getDefaultMaterial());

        root->setCore(geo);
    }

    return root;

}

bool STLSceneFileType::isASCII(std::istream &is, const Char8* fileNameOrExtension) const
{
    std::string theAbsName = fileNameOrExtension;
    std::string theExtension;
    int nameLength = theAbsName.length();
    int separator  = theAbsName.rfind(".", nameLength);
    theExtension = theAbsName.substr(separator+1, nameLength-separator-1);

    for (int i = 0; i < theExtension.length(); i++)
        theExtension[i] = tolower(theExtension[i]);

    if (theExtension.compare("stla") == 0)
        return true;
    else if (theExtension.compare("stlb") == 0)
        return false;
    else
    {
        // we have to check if our file start with "solid"
        std::string theCheck;
        is >> theCheck;
        for (int i = 0; i< theCheck.length(); i++)
                theCheck[i] = tolower(theCheck[i]);

        is.seekg( 0, std::ios_base::beg );

        return (theCheck.compare("solid") == 0);
    }
}

bool STLSceneFileType::readASCII(std::istream &is, STLFaceList& theFaces, std::string& theName) const
{
    FDEBUG(("STLSceneFileType::readASCII:\n"));

    char token[9];
    int finished = 0;
    int parseResult = 0;
    UInt32 i;

    if(!is)
    return false;

    while ((finished == 0) && (! (is.eof())))
    {
        //SceneFileHandler::the().updateReadProgress();
        memset(token, ' ', 9);
        is >> token;
        for (i = 0; i < strlen(token); i++)
        {
            token[i] = tolower(token[i]);
        }
        if (strcmp(token, "solid") == 0)
        {
            char tmp = is.get();
            if (tmp == ' ')
            is >> theName;
        }
        else if (strcmp(token, "facet") == 0)
        {
            STLFace tmpFace;
            is >> token >> tmpFace.nx >> tmpFace.ny >> tmpFace.nz;
            is >> token >> token;// skip "outer loop"
            is >> token >> tmpFace.v1x >> tmpFace.v1y >> tmpFace.v1z;
            is >> token >> tmpFace.v2x >> tmpFace.v2y >> tmpFace.v2z;
            is >> token >> tmpFace.v3x >> tmpFace.v3y >> tmpFace.v3z;
            is >> token;// skip "endloop"
            is >> token;// skip "endfacet"
            theFaces.push_back(tmpFace);
        }
        else if (strcmp(token, "endsolid") == 0)
        {
            finished = 1;
            return true;
        }
    }
    return false;
}

bool STLSceneFileType::readBinary(std::istream &is, STLFaceList& theFaces, std::string& theName) const
{
    FDEBUG(("STLSceneFileType::readBinary:\n"));

    if(!is)
        return false;

    bool bigEndian = osgIsBigEndian();

    int HeaderSize = 84;    // 80bytes for name and 4 bytes for number of facets
    char name[80] = "";
    is.read( &name[0], 80 );
    theName = name;
    is.read(&name[0], 4); // skip the number of facets
    while (!is.eof())
    {
        //SceneFileHandler::the().updateReadProgress();
        // a binary facet block has 50 bytes
        STLFace tmpFace;

        tmpFace.nx = readFloat(is, bigEndian);
        tmpFace.ny = readFloat(is, bigEndian);
        tmpFace.nz = readFloat(is, bigEndian);

        tmpFace.v1x = readFloat(is, bigEndian);
        tmpFace.v1y = readFloat(is, bigEndian);
        tmpFace.v1z = readFloat(is, bigEndian);

        tmpFace.v2x = readFloat(is, bigEndian);
        tmpFace.v2y = readFloat(is, bigEndian);
        tmpFace.v2z = readFloat(is, bigEndian);

        tmpFace.v3x = readFloat(is, bigEndian);
        tmpFace.v3y = readFloat(is, bigEndian);
        tmpFace.v3z = readFloat(is, bigEndian);

        theFaces.push_back(tmpFace);
        char buffer[2]; // offset filler
        is.read (&buffer[0], 2);
    }
    return true;
}

Real32 STLSceneFileType::readFloat(std::istream& is, bool bigEndian) const
{
    Real32 result = 0;

    unsigned char buffer[4];
    is.read (reinterpret_cast<char*>(&buffer[0]), 4);
    Real32* tmp = reinterpret_cast<Real32*>(&buffer);
    result = *tmp;
    if (bigEndian)
        result = osgNetToHost(result);
    return result;
}

#ifdef __sgi
#pragma reset woff 1209
#endif

/******************************
*protected
******************************/

/******************************
*private
******************************/

/***************************
*instance methodes
***************************/

/***************************
*public
***************************/

/**constructors & destructors**/

STLSceneFileType::STLSceneFileType(const Char8  *suffixArray[],
                                        UInt16  suffixByteCount,
                                        bool    override,
                                        UInt32  overridePriority,
                                        UInt32  flags) :
    SceneFileType(suffixArray,
                suffixByteCount,
                override,
                overridePriority,
                flags)
{
}

STLSceneFileType &STLSceneFileType::the(void)
{
    return _the;
}

const Char8 *STLSceneFileType::getName(void) const
{
    return "Stereolithography Geometry";
}

STLSceneFileType::~STLSceneFileType (void )
{
    return;
}

<|endoftext|>


<commit_before>/*
 * Copyright 2016 The Cartographer 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.
 */

#include "cartographer/mapping/detect_floors.h"

#include <algorithm>
#include <fstream>
#include <vector>

#include "Eigen/Core"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping {

namespace {

// A union-find structure for assigning levels to 'spans' in the trajectory
// implemented as a disjoint-set forest:
// https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests
// TODO(hrapp): We use this elsewhere. Pull out into a common place.
using Levels = std::map<int, int>;

constexpr double kMaxShortSpanLengthMeters = 25.;
constexpr double kLevelHeightMeters = 2.5;
constexpr double kMinLevelSeparationMeters = 1.;

// Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'.
struct Span {
  int start_index;
  int end_index;
  std::vector<double> z_values;

  bool operator<(const Span& other) const {
    return std::forward_as_tuple(start_index, end_index) <
           std::forward_as_tuple(other.start_index, other.end_index);
  }
};

// Union-find implementation for classifying spans into levels.
int LevelFind(const int i, const Levels& levels) {
  auto it = levels.find(i);
  CHECK(it != levels.end());
  if (it->first == it->second) {
    return it->second;
  }
  return LevelFind(it->second, levels);
}

void LevelUnion(int i, int j, Levels* levels) {
  const int repr_i = LevelFind(i, *levels);
  const int repr_j = LevelFind(j, *levels);
  (*levels)[repr_i] = repr_j;
}

void InsertSorted(const double val, std::vector<double>* vals) {
  vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val);
}

double Median(const std::vector<double>& sorted) {
  CHECK(!sorted.empty());
  return sorted.at(sorted.size() / 2);
}

// Cut the trajectory at jumps in z. A new span is started when the current
// node's z differes by more than kLevelHeightMeters from the median z values.
std::vector<Span> SliceByAltitudeChange(const proto::Trajectory& trajectory) {
  CHECK_GT(trajectory.node_size(), 0);

  std::vector<Span> spans;
  spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}});
  for (int i = 1; i < trajectory.node_size(); ++i) {
    const auto& node = trajectory.node(i);
    const double z = node.pose().translation().z();
    if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) {
      spans.push_back(Span{i, i, {}});
    }
    InsertSorted(z, &spans.back().z_values);
    spans.back().end_index = i + 1;
  }
  return spans;
}

// Returns the length of 'span' in meters.
double SpanLength(const proto::Trajectory& trajectory, const Span& span) {
  double length = 0;
  for (int i = span.start_index + 1; i < span.end_index; ++i) {
    const auto a =
        transform::ToEigen(trajectory.node(i - 1).pose().translation());
    const auto b = transform::ToEigen(trajectory.node(i).pose().translation());
    length += (a - b).head<2>().norm();
  }
  return length;
}

// True if 'span' is considered to be short, i.e. not interesting on its own,
// but should be folded into the levels before and after entering it.
bool IsShort(const proto::Trajectory& trajectory, const Span& span) {
  return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters;
}

// Merges all 'spans' that have similar median z value into the same level.
void GroupSegmentsByAltitude(const proto::Trajectory& trajectory,
                             const std::vector<Span>& spans, Levels* levels) {
  for (size_t i = 0; i < spans.size(); ++i) {
    for (size_t j = i + 1; j < spans.size(); ++j) {
      if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) <
          kMinLevelSeparationMeters) {
        LevelUnion(i, j, levels);
      }
    }
  }
}

std::vector<Floor> FindFloors(const proto::Trajectory& trajectory,
                              const std::vector<Span>& spans,
                              const Levels& levels) {
  std::map<int, std::vector<Span>> level_spans;

  // Initialize the levels to start out with only long spans.
  for (size_t i = 0; i < spans.size(); ++i) {
    const Span& span = spans[i];
    if (!IsShort(trajectory, span)) {
      level_spans[LevelFind(i, levels)].push_back(span);
    }
  }

  for (size_t i = 0; i < spans.size(); ++i) {
    const Span& span = spans[i];
    if (!IsShort(trajectory, span)) {
      continue;
    }

    // If we have a long piece on this floor already, merge this short piece
    // into it.
    int level = LevelFind(i, levels);
    if (!level_spans[level].empty()) {
      level_spans[level].push_back(span);
      continue;
    }

    // Otherwise, add this short piece to the level before and after it. It is
    // likely some intermediate level on stairs.
    size_t index = i - 1;
    if (index < spans.size()) {
      level_spans[LevelFind(index, levels)].push_back(span);
    }
    index = i + 1;
    if (index < spans.size()) {
      level_spans[LevelFind(index, levels)].push_back(span);
    }
  }

  // Convert the level_spans structure to 'Floor'.
  std::vector<Floor> floors;
  for (auto& level : level_spans) {
    if (level.second.empty()) {
      continue;
    }

    std::vector<double> z_values;
    std::sort(level.second.begin(), level.second.end());
    floors.emplace_back();
    for (const auto& span : level.second) {
      if (!IsShort(trajectory, span)) {
        // To figure out the median height of this floor, we only care for the
        // long pieces that are guaranteed to be in the structure. This is a
        // heuristic to leave out intermediate (short) levels.
        z_values.insert(z_values.end(), span.z_values.begin(),
                        span.z_values.end());
      }
      floors.back().timespans.push_back(Timespan{
          common::FromUniversal(trajectory.node(span.start_index).timestamp()),
          common::FromUniversal(
              trajectory.node(span.end_index - 1).timestamp())});
    }
    std::sort(z_values.begin(), z_values.end());
    floors.back().z = Median(z_values);
  }
  return floors;
}

}  // namespace

std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory) {
  const std::vector<Span> spans = SliceByAltitudeChange(trajectory);
  Levels levels;
  for (size_t i = 0; i < spans.size(); ++i) {
    levels[i] = i;
  }
  GroupSegmentsByAltitude(trajectory, spans, &levels);

  std::vector<Floor> floors = FindFloors(trajectory, spans, levels);
  std::sort(floors.begin(), floors.end(),
            [](const Floor& a, const Floor& b) { return a.z < b.z; });
  return floors;
}

}  // namespace mapping
}  // namespace cartographer
<commit_msg>Fix crashes in median() if all spans in a level are short (#1668)<commit_after>/*
 * Copyright 2016 The Cartographer 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.
 */

#include "cartographer/mapping/detect_floors.h"

#include <algorithm>
#include <fstream>
#include <vector>

#include "Eigen/Core"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping {

namespace {

// A union-find structure for assigning levels to 'spans' in the trajectory
// implemented as a disjoint-set forest:
// https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests
// TODO(hrapp): We use this elsewhere. Pull out into a common place.
using Levels = std::map<int, int>;

constexpr double kMaxShortSpanLengthMeters = 25.;
constexpr double kLevelHeightMeters = 2.5;
constexpr double kMinLevelSeparationMeters = 1.;

// Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'.
struct Span {
  int start_index;
  int end_index;
  std::vector<double> z_values;

  bool operator<(const Span& other) const {
    return std::forward_as_tuple(start_index, end_index) <
           std::forward_as_tuple(other.start_index, other.end_index);
  }
};

// Union-find implementation for classifying spans into levels.
int LevelFind(const int i, const Levels& levels) {
  auto it = levels.find(i);
  CHECK(it != levels.end());
  if (it->first == it->second) {
    return it->second;
  }
  return LevelFind(it->second, levels);
}

void LevelUnion(int i, int j, Levels* levels) {
  const int repr_i = LevelFind(i, *levels);
  const int repr_j = LevelFind(j, *levels);
  (*levels)[repr_i] = repr_j;
}

void InsertSorted(const double val, std::vector<double>* vals) {
  vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val);
}

double Median(const std::vector<double>& sorted) {
  CHECK(!sorted.empty());
  return sorted.at(sorted.size() / 2);
}

// Cut the trajectory at jumps in z. A new span is started when the current
// node's z differes by more than kLevelHeightMeters from the median z values.
std::vector<Span> SliceByAltitudeChange(const proto::Trajectory& trajectory) {
  CHECK_GT(trajectory.node_size(), 0);

  std::vector<Span> spans;
  spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}});
  for (int i = 1; i < trajectory.node_size(); ++i) {
    const auto& node = trajectory.node(i);
    const double z = node.pose().translation().z();
    if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) {
      spans.push_back(Span{i, i, {}});
    }
    InsertSorted(z, &spans.back().z_values);
    spans.back().end_index = i + 1;
  }
  return spans;
}

// Returns the length of 'span' in meters.
double SpanLength(const proto::Trajectory& trajectory, const Span& span) {
  double length = 0;
  for (int i = span.start_index + 1; i < span.end_index; ++i) {
    const auto a =
        transform::ToEigen(trajectory.node(i - 1).pose().translation());
    const auto b = transform::ToEigen(trajectory.node(i).pose().translation());
    length += (a - b).head<2>().norm();
  }
  return length;
}

// True if 'span' is considered to be short, i.e. not interesting on its own,
// but should be folded into the levels before and after entering it.
bool IsShort(const proto::Trajectory& trajectory, const Span& span) {
  return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters;
}

// Merges all 'spans' that have similar median z value into the same level.
void GroupSegmentsByAltitude(const proto::Trajectory& trajectory,
                             const std::vector<Span>& spans, Levels* levels) {
  for (size_t i = 0; i < spans.size(); ++i) {
    for (size_t j = i + 1; j < spans.size(); ++j) {
      if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) <
          kMinLevelSeparationMeters) {
        LevelUnion(i, j, levels);
      }
    }
  }
}

std::vector<Floor> FindFloors(const proto::Trajectory& trajectory,
                              const std::vector<Span>& spans,
                              const Levels& levels) {
  std::map<int, std::vector<Span>> level_spans;

  // Initialize the levels to start out with only long spans.
  for (size_t i = 0; i < spans.size(); ++i) {
    const Span& span = spans[i];
    if (!IsShort(trajectory, span)) {
      level_spans[LevelFind(i, levels)].push_back(span);
    }
  }

  for (size_t i = 0; i < spans.size(); ++i) {
    const Span& span = spans[i];
    if (!IsShort(trajectory, span)) {
      continue;
    }

    // If we have a long piece on this floor already, merge this short piece
    // into it.
    int level = LevelFind(i, levels);
    if (!level_spans[level].empty()) {
      level_spans[level].push_back(span);
      continue;
    }

    // Otherwise, add this short piece to the level before and after it. It is
    // likely some intermediate level on stairs.
    size_t index = i - 1;
    if (index < spans.size()) {
      level_spans[LevelFind(index, levels)].push_back(span);
    }
    index = i + 1;
    if (index < spans.size()) {
      level_spans[LevelFind(index, levels)].push_back(span);
    }
  }

  // Convert the level_spans structure to 'Floor'.
  std::vector<Floor> floors;
  for (auto& level : level_spans) {
    if (level.second.empty()) {
      continue;
    }

    std::vector<double> z_values;
    std::sort(level.second.begin(), level.second.end());
    floors.emplace_back();
    for (const auto& span : level.second) {
      if (!IsShort(trajectory, span)) {
        // To figure out the median height of this floor, we only care for the
        // long pieces that are guaranteed to be in the structure. This is a
        // heuristic to leave out intermediate (short) levels.
        z_values.insert(z_values.end(), span.z_values.begin(),
                        span.z_values.end());
      }
      floors.back().timespans.push_back(Timespan{
          common::FromUniversal(trajectory.node(span.start_index).timestamp()),
          common::FromUniversal(
              trajectory.node(span.end_index - 1).timestamp())});
    }
    if (!z_values.empty()) {
      std::sort(z_values.begin(), z_values.end());
      floors.back().z = Median(z_values);
    } else {
      LOG(ERROR) << "All spans in level are short";
      floors.pop_back();
    }
  }
  return floors;
}

}  // namespace

std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory) {
  const std::vector<Span> spans = SliceByAltitudeChange(trajectory);
  Levels levels;
  for (size_t i = 0; i < spans.size(); ++i) {
    levels[i] = i;
  }
  GroupSegmentsByAltitude(trajectory, spans, &levels);

  std::vector<Floor> floors = FindFloors(trajectory, spans, levels);
  std::sort(floors.begin(), floors.end(),
            [](const Floor& a, const Floor& b) { return a.z < b.z; });
  return floors;
}

}  // namespace mapping
}  // namespace cartographer
<|endoftext|>


<commit_before>#include	<cmath>
#include	<cstdlib>
#include	<iostream>
#include	<iomanip>
#include	<fstream>
#include	<sstream>
#include	<vector>
#include	<exception>

#include	"global.h"
#include	"multigrid.h"

// A code to perform reconstruction, using the lowest order algorithm.
//
// Author:	Martin White	(UCB/LBNL)
// Written:	20-Apr-2015
// Modified:	22-Apr-2015	(Take two random catalogs)
//
//
// This code uses a standard multigrid technique to compute the
// displacements given an observed density field and then move the
// objects and "randoms" back along the displacement vector.
//
// Want to get rid of continual allocation and deallocation of arrays.
// I think we just need two arrays at each level, could call them A & B,
// or V1 and V2.
//
// Could make this use double precision throughout.  Memory isn't a
// major problem so far.
//

// Global variables.
float	bias=1,beta=0;
struct	Box box;

void    myexit(const int flag)
{
  std::cout.flush();
  std::cerr.flush();
  exit(flag);
}

void    myexception(const std::exception& e)
{
  std::cout<<": Exception: "<<e.what()<<std::endl;
  std::cout.flush();
  std::cerr.flush();
  exit(1);
}

int	main(int argc, char **argv)
{
  float Rf;
  if (argc!=7) {
    std::cout<<"Usage: recon <data-file> <random-file> <random-file>"
             <<" <bias> <f-growth> <R-filter>"<<std::endl;
    myexit(1);
  }
  bias = atof(argv[4]);		// Sets global variable.
  beta = atof(argv[5])/bias;	// Sets global variable.
  Rf   = atof(argv[6]);
  LCDM lcdm(0.30);		// Change OmegaM here if necessary.

#ifdef	TESTMG
  // Make a cosine wave (only in x-direction) and solve for it with
  // beta=0.  Used to test the MG solver convergence on a simple problem.
  box.L=1.25;  box.ctr[0]=box.ctr[1]=box.ctr[2]=0.6;
  const float dt=2*M_PI/Ng;
  std::vector<float> src(Ng*Ng*Ng);
  for (int ix=0; ix<Ng; ++ix)
    for (int iy=0; iy<Ng; ++iy)
      for (int iz=0; iz<Ng; ++iz)
        src[Ng*Ng*ix+Ng*iy+iz] = cos(ix*dt);
  bias = 1; beta = 0;
  std::vector<float> ans = MultiGrid::fmg(src,Ng);
  for (int ix=0; ix<Ng; ++ix)
    std::cout<<std::fixed<<std::setprecision(6)
             <<ix<<" "<<cos(ix*dt)/(2*M_PI*2*M_PI)
             <<" "<<ans[Ng*Ng*ix+Ng*0   +Ng/4]
             <<" "<<ans[Ng*Ng*ix+Ng*Ng/2+0]
             <<" "<<ans[Ng*Ng*ix+Ng*0   +Ng/2]<<std::endl;
  return(0);
#endif

  // Read the data and figure out the 3D positions and enclosing box.
  std::vector<struct particle> D = read_data(argv[1],lcdm);
  std::vector<struct particle> R1= read_data(argv[2],lcdm);
  std::vector<struct particle> R2= read_data(argv[3],lcdm);
  std::cout<<"# Read "<<std::setw(10)<<D.size()
           <<" objects from "<<argv[1]<<std::endl;
  std::cout<<"# Read "<<std::setw(10)<<R1.size()
           <<" randoms from "<<argv[2]<<std::endl;
  std::cout<<"# Read "<<std::setw(10)<<R2.size()
           <<" randoms from "<<argv[3]<<std::endl;
  remap_pos(D,R1,R2);
  std::cout<<"# Enclosing survey in a box of side "<<box.L<<" Mpc/h."
           <<std::endl;
  std::cout<<"# Grid/mesh size is "<<box.L/Ng<<" Mpc/h"
           <<" and filter scale is "<<Rf<<" Mpc/h."
           <<std::endl;

  write_data(D ,"data_raw.xyzw");
  write_data(R2,"rand_raw.xyzw");

  // Make the density (contrast) grid and solve for the
  // displacement potential, phi.
  std::vector<float> delta = make_grid(D,R1,Rf);
  std::vector<float> phi   = MultiGrid::fmg(delta,Ng);

  // Shift the particles and randoms back -- if you want to not enhance
  // the line-of-sight shift for the randoms you need to change beta before
  // calling shift_obj.
  shift_obj(D ,phi);
  shift_obj(R2,phi);

  write_data(D ,"data_rec.xyzw");
  write_data(R2,"rand_rec.xyzw");

  return(0);
}
<commit_msg>Add an option to skip the writing of the "raw" catalogs.<commit_after>#include	<cmath>
#include	<cstdlib>
#include	<iostream>
#include	<iomanip>
#include	<fstream>
#include	<sstream>
#include	<vector>
#include	<exception>

#include	"global.h"
#include	"multigrid.h"

// A code to perform reconstruction, using the lowest order algorithm.
//
// Author:	Martin White	(UCB/LBNL)
// Written:	20-Apr-2015
// Modified:	22-Apr-2015	(Take two random catalogs)
//
//
// This code uses a standard multigrid technique to compute the
// displacements given an observed density field and then move the
// objects and "randoms" back along the displacement vector.
//
// Want to get rid of continual allocation and deallocation of arrays.
// I think we just need two arrays at each level, could call them A & B,
// or V1 and V2.
//
// Could make this use double precision throughout.  Memory isn't a
// major problem so far.
//

// Global variables.
float	bias=1,beta=0;
struct	Box box;

void    myexit(const int flag)
{
  std::cout.flush();
  std::cerr.flush();
  exit(flag);
}

void    myexception(const std::exception& e)
{
  std::cout<<": Exception: "<<e.what()<<std::endl;
  std::cout.flush();
  std::cerr.flush();
  exit(1);
}

int	main(int argc, char **argv)
{
  float Rf;
  if (argc!=7) {
    std::cout<<"Usage: recon <data-file> <random-file> <random-file>"
             <<" <bias> <f-growth> <R-filter>"<<std::endl;
    myexit(1);
  }
  bias = atof(argv[4]);		// Sets global variable.
  beta = atof(argv[5])/bias;	// Sets global variable.
  Rf   = atof(argv[6]);
  LCDM lcdm(0.30);		// Change OmegaM here if necessary.

#ifdef	TESTMG
  // Make a cosine wave (only in x-direction) and solve for it with
  // beta=0.  Used to test the MG solver convergence on a simple problem.
  box.L=1.25;  box.ctr[0]=box.ctr[1]=box.ctr[2]=0.6;
  const float dt=2*M_PI/Ng;
  std::vector<float> src(Ng*Ng*Ng);
  for (int ix=0; ix<Ng; ++ix)
    for (int iy=0; iy<Ng; ++iy)
      for (int iz=0; iz<Ng; ++iz)
        src[Ng*Ng*ix+Ng*iy+iz] = cos(ix*dt);
  bias = 1; beta = 0;
  std::vector<float> ans = MultiGrid::fmg(src,Ng);
  for (int ix=0; ix<Ng; ++ix)
    std::cout<<std::fixed<<std::setprecision(6)
             <<ix<<" "<<cos(ix*dt)/(2*M_PI*2*M_PI)
             <<" "<<ans[Ng*Ng*ix+Ng*0   +Ng/4]
             <<" "<<ans[Ng*Ng*ix+Ng*Ng/2+0]
             <<" "<<ans[Ng*Ng*ix+Ng*0   +Ng/2]<<std::endl;
  return(0);
#endif

  // Read the data and figure out the 3D positions and enclosing box.
  std::vector<struct particle> D = read_data(argv[1],lcdm);
  std::vector<struct particle> R1= read_data(argv[2],lcdm);
  std::vector<struct particle> R2= read_data(argv[3],lcdm);
  std::cout<<"# Read "<<std::setw(10)<<D.size()
           <<" objects from "<<argv[1]<<std::endl;
  std::cout<<"# Read "<<std::setw(10)<<R1.size()
           <<" randoms from "<<argv[2]<<std::endl;
  std::cout<<"# Read "<<std::setw(10)<<R2.size()
           <<" randoms from "<<argv[3]<<std::endl;
  remap_pos(D,R1,R2);
  std::cout<<"# Enclosing survey in a box of side "<<box.L<<" Mpc/h."
           <<std::endl;
  std::cout<<"# Grid/mesh size is "<<box.L/Ng<<" Mpc/h"
           <<" and filter scale is "<<Rf<<" Mpc/h."
           <<std::endl;

#ifndef	SKIPRAW
  write_data(D ,"data_raw.xyzw");
  write_data(R2,"rand_raw.xyzw");
#endif

  // Make the density (contrast) grid and solve for the
  // displacement potential, phi.
  std::vector<float> delta = make_grid(D,R1,Rf);
  std::vector<float> phi   = MultiGrid::fmg(delta,Ng);

  // Shift the particles and randoms back -- if you want to not enhance
  // the line-of-sight shift for the randoms you need to change beta before
  // calling shift_obj.
  shift_obj(D ,phi);
  shift_obj(R2,phi);

  write_data(D ,"data_rec.xyzw");
  write_data(R2,"rand_rec.xyzw");

  return(0);
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright (c) 2014-2017 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include <iostream>
#include <chrono>
#include <random>

#define ETL_COUNTERS
#define ETL_COUNTERS_VERBOSE

#include "etl/etl.hpp"

typedef std::chrono::high_resolution_clock timer_clock;
typedef std::chrono::milliseconds milliseconds;

float fake = 0;

/*
 *
 * Current values are (alloc/gpu_to_cpu/cpu_to_gpu):
 * Simple: 3 / 2 / 0 (Optimal!)
 * Basic: 50 / 30 / 10
 * Sub: 960 / 640 / 160
 */

void simple(){
    etl::dyn_matrix<float, 2> A(4096, 4096);
    etl::dyn_matrix<float, 2> B(4096, 4096);
    etl::dyn_matrix<float, 2> C(4096, 4096);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Simple" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        C = A * B;
        fake += etl::mean(C);
    }

    etl::dump_counters();
}

void basic(){
    etl::dyn_matrix<float, 2> A(4096, 4096);
    etl::dyn_matrix<float, 2> B(4096, 4096);
    etl::dyn_matrix<float, 2> C(4096, 4096);
    etl::dyn_matrix<float, 2> D(4096, 4096);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);
    D = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Basic" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        C = A * B * B;
        D += C;
        D *= 1.1;
        fake += etl::mean(D);
    }
    etl::dump_counters();
}

void sub(){
    etl::dyn_matrix<float, 3> A(16, 2048, 2048);
    etl::dyn_matrix<float, 3> B(16, 2048, 2048);
    etl::dyn_matrix<float, 3> C(16, 2048, 2048);
    etl::dyn_matrix<float, 3> D(16, 2048, 2048);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);
    D = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Sub" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        for (size_t k = 0; k < 16; ++k) {
            C(k) = A(k) * B(k) * B(k);
            D(k) += C(k);
            D(k) *= 1.1;
            fake += etl::mean(D(k));
        }
    }

    etl::dump_counters();
}

int main(){
    auto start_time = timer_clock::now();

    simple();
    basic();
    sub();

    auto end_time = timer_clock::now();
    auto duration = std::chrono::duration_cast<milliseconds>(end_time - start_time);

    std::cout << "duration: " << duration.count() << "ms" << std::endl;

    return (int) fake;
}
<commit_msg>Review the test<commit_after>//=======================================================================
// Copyright (c) 2014-2017 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include <iostream>
#include <chrono>
#include <random>

#define ETL_COUNTERS
#define ETL_COUNTERS_VERBOSE

#include "etl/etl.hpp"

typedef std::chrono::high_resolution_clock timer_clock;
typedef std::chrono::milliseconds milliseconds;

float fake = 0;

/*
 *
 * Current values are (alloc/gpu_to_cpu/cpu_to_gpu):
 * Simple: 3 / 2 / 0 (Optimal!)
 * Basic: 50 / 30 / 10
 * Sub: 960 / 640 / 160
 */

void simple(){
    etl::dyn_matrix<float, 2> A(4096, 4096);
    etl::dyn_matrix<float, 2> B(4096, 4096);
    etl::dyn_matrix<float, 2> C(4096, 4096);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Simple" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        C = A * B;
        fake += etl::mean(C);
    }

    etl::dump_counters();
}

void basic(){
    etl::dyn_matrix<float, 2> A(4096, 4096);
    etl::dyn_matrix<float, 2> B(4096, 4096);
    etl::dyn_matrix<float, 2> C(4096, 4096);
    etl::dyn_matrix<float, 2> D(4096, 4096);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);
    D = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Basic" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        C = A * B * B;
        D = A * trans(A);
        D *= 1.1;
        D += C;
        fake += etl::mean(D);
    }
    etl::dump_counters();
}

void sub(){
    etl::dyn_matrix<float, 3> A(16, 2048, 2048);
    etl::dyn_matrix<float, 3> B(16, 2048, 2048);
    etl::dyn_matrix<float, 3> C(16, 2048, 2048);
    etl::dyn_matrix<float, 3> D(16, 2048, 2048);

    A = etl::normal_generator<float>(1.0, 0.0);
    B = etl::normal_generator<float>(1.0, 0.0);
    C = etl::normal_generator<float>(1.0, 0.0);
    D = etl::normal_generator<float>(1.0, 0.0);

    etl::reset_counters();

    std::cout << "Sub" << std::endl;

    for (size_t i = 0; i < 10; ++i) {
        for (size_t k = 0; k < 16; ++k) {
            C(k) = A(k) * B(k) * B(k);
            D(k) += C(k);
            D(k) *= 1.1;
            fake += etl::mean(D(k));
        }
    }

    etl::dump_counters();
}

int main(){
    auto start_time = timer_clock::now();

    simple();
    basic();
    sub();

    auto end_time = timer_clock::now();
    auto duration = std::chrono::duration_cast<milliseconds>(end_time - start_time);

    std::cout << "duration: " << duration.count() << "ms" << std::endl;

    return (int) fake;
}
<|endoftext|>


<commit_before>/*
    Copyright (C) 2000 by Samuel Humphreys

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

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "cssysdef.h"
#include "protex3d.h"
#include "ivideo/graph2d.h"
#include "soft_txt.h"
#include "tcache.h"
#include "csgfx/memimage.h"
#include "csgfx/rgbpixel.h"
#include "ivideo/texture.h"
#include "iutil/objreg.h"
#include "ivaria/reporter.h"

SCF_IMPLEMENT_FACTORY (csSoftProcTexture3D)

SCF_IMPLEMENT_IBASE_EXT (csSoftProcTexture3D)
  SCF_IMPLEMENTS_EMBEDDED_INTERFACE (iSoftProcTexture)
SCF_IMPLEMENT_IBASE_EXT_END

SCF_IMPLEMENT_EMBEDDED_IBASE (csSoftProcTexture3D::eiSoftProcTexture)
  SCF_IMPLEMENTS_INTERFACE (iSoftProcTexture)
SCF_IMPLEMENT_EMBEDDED_IBASE_END

csSoftProcTexture3D::csSoftProcTexture3D (iBase *iParent)
  : csGraphics3DSoftwareCommon (iParent)
{
  SCF_CONSTRUCT_EMBEDDED_IBASE(scfiSoftProcTexture);
  is_for_procedural_textures = true;
  soft_tex_mm = NULL;
  parent_tex_mm = NULL;
  reprepare = false;
}

csSoftProcTexture3D::~csSoftProcTexture3D ()
{
  if (sharing)
  {
    // We are sharing the cache and texture manager
    tcache = NULL;
    texman = NULL;
  }
}

void csSoftProcTexture3D::Print (csRect *area)
{
  csGraphics3DSoftwareCommon::Print (area);

  if (reprepare)
    soft_tex_mm->ReprepareProcTexture ();

  // As we've printed something we need to uncache it so the effects will be
  // registered next frame.
  if (parent_tex_mm)
    main_tcache->uncache_texture (0, (iTextureHandle*)parent_tex_mm);
}

bool csSoftProcTexture3D::Prepare (csTextureManagerSoftware *main_texman,
  csTextureHandleSoftware *tex_mm, void *buffer,  uint8 *bitmap)
{
  csGraphics3DSoftwareCommon* parent_g3d = main_texman->G3D;
  iGraphics2D *g2d = parent_g3d->GetDriver2D ();
  object_reg = parent_g3d->object_reg;

  main_tcache = parent_g3d->tcache;
  parent_tex_mm = tex_mm;
  parent_tex_mm->GetMipMapDimensions (0, width, height);
  int flags = parent_tex_mm->GetFlags ();
  bool use8bit = (flags & CS_TEXTURE_PROC_ALONE_HINT) ==
                     CS_TEXTURE_PROC_ALONE_HINT;
  // Cases:
  // Current Display..Texture Manager..tex_mm flags.....Reprepare
  //       8bit         main             none               no
  //     15/16bit       main             none               yes
  //     15/16bit       proc   CS_TEXTURE_PROC_ALONE_HINT   no
  //       32bit        main             none               yes
  //       32bit        proc   CS_TEXTURE_PROC_ALONE_HINT   no
  //

  if (main_texman->pfmt.PixelBytes == 1)
  {
    sharing = true;
    reprepare = false;
    // Global palette
    G2D = g2d->CreateOffScreenCanvas (width, height, bitmap, use8bit,
		     &main_texman->pfmt,  &main_texman->cmap.palette[0], 256);
    if (!G2D) return false;

    SharedInitialize (parent_g3d);
    if (!Open () || !SharedOpen ())
      return false;

#ifdef CS_DEBUG
    printf ("8bit procedural texture\n");
#endif
  }
  else
  {
    if (!use8bit)
    {
      sharing = true;
      reprepare = true;
      soft_tex_mm = tex_mm;

      G2D = g2d->CreateOffScreenCanvas (width, height, buffer, false,
        &main_texman->pfmt, parent_tex_mm->GetColorMap (), 256);

      SharedInitialize (parent_g3d);
      if (!Open () || !SharedOpen ())
	return false;

#ifdef CS_DEBUG
      printf ("%sbit procedural texture\n",
        main_texman->pfmt.PixelBytes == 2 ? "16" : "32");
#endif
    }
    else
    {
      reprepare = false;
      G2D = g2d->CreateOffScreenCanvas (width, height, bitmap, true,
        &main_texman->pfmt, parent_tex_mm->GetColorMap (), 256);
      if (!G2D) return false;

      if (!main_texman->GetFirst8bitProcTexture ())
      {
        sharing = false;
        // We are the first procedural texture utilising a dedicated
        // procedural texture manager working in 8bit
        NewInitialize ();
        if (!Open () || !NewOpen ())
          return false;
        // Inform each texture manager aboout the other
        texman->SetMainTextureManager (main_texman);
        main_texman->SetProcTextureManager (texman);
        texman->ResetPalette ();
#ifdef CS_DEBUG
        printf ("Preparing dedicated procedural texture manager\n");
#endif
        // Initialize the texture manager
        int r,g,b;
        for (r = 0; r < 8; r++)
          for (g = 0; g < 8; g++)
            for (b = 0; b < 4; b++)
              texman->ReserveColor (r * 32, g * 32, b * 64);
        texman->PrepareTextures ();
      }
      else
      {
        sharing = true;
        SharedInitialize (main_texman->GetFirst8bitProcTexture ());
        if (!Open () || !SharedOpen ())
          return false;
      }

      if (buffer)
      {
        // In order to keep the palette synchronised
        iImage *im = (iImage*) new csImageMemory (width, height,
          (csRGBpixel *) buffer, false);
	  csTextureHandleSoftware *dummy =
          (csTextureHandleSoftware *)texman->RegisterTexture (im, CS_TEXTURE_2D | CS_TEXTURE_PROC);
        dummy->Prepare();
	dummy->DecRef();
      }

#ifdef CS_DEBUG
      printf ("%s/8bit procedural texture\n",
        main_texman->pfmt.PixelBytes == 2 ? "16" : "32");
#endif
    }
  }
  return true;
}

// The entry point for other than software graphics drivers..
iTextureHandle *csSoftProcTexture3D::CreateOffScreenRenderer
  (iGraphics3D *parent_g3d, iGraphics3D* g3d_partner, int width, int height,
   void *buffer, csPixelFormat *ipfmt, int flags)
{
  // Always in 32bit
  // Here we create additional images for this texture which are registered as
  // procedural textures with our own texture manager.  This way if the
  // procedural texture is written to with itself, this texture manager will
  // know about it and act accordingly.  This is done for both alone and
  // sharing procedural textures.  In the case of the alone procedural texture
  // it will never be utilised unless a sharing procedural texture is
  // introduced into the system in which case the previous alone procedural
  // textures are all converted to sharing ones.

  // If this is a sharing procedural texture then the texture needs repreparing
  // each update.
  if ((flags & CS_TEXTURE_PROC_ALONE_HINT) !=  CS_TEXTURE_PROC_ALONE_HINT)
    reprepare = true;

  G2D = parent_g3d->GetDriver2D ()->CreateOffScreenCanvas (
    width, height, buffer, false, ipfmt, NULL, 0);

  if (!G2D)
  {
    csReport (object_reg, CS_REPORTER_SEVERITY_ERROR,
      	"crystalspace.video.software.protex3d",
	"Error opening Graphics2D texture context.");
    return NULL;
  }

  if (g3d_partner)
  {
    sharing = true;
    partner = (csGraphics3DSoftwareCommon*)g3d_partner;
    SharedInitialize (partner);
    if (!Open () || !SharedOpen ())
      return NULL;
  }
  else
  {
    sharing = false;
    NewInitialize ();
    if (!Open () || !NewOpen ())
      return NULL;
  }

  // Register our own buffer as a procedural texture.
  // The texture handles GetProcTextureInterface () is never called so
  // no additional interfaces are created.

  // We set the 'destroy' parameter for csImageMemory to false as these buffers
  // are destroyed else where.
  iImage *tex_image = new csImageMemory (width, height,
					 (csRGBpixel*)buffer, false);

  soft_tex_mm = (csTextureHandleSoftware *)
        texman->RegisterTexture (tex_image, CS_TEXTURE_PROC | CS_TEXTURE_2D);
  soft_tex_mm->Prepare ();

  // Return the software texture managers handle for this procedural texture.
  return soft_tex_mm;
}
<commit_msg>decref'ed image used for palette synchronization<commit_after>/*
    Copyright (C) 2000 by Samuel Humphreys

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

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "cssysdef.h"
#include "protex3d.h"
#include "ivideo/graph2d.h"
#include "soft_txt.h"
#include "tcache.h"
#include "csgfx/memimage.h"
#include "csgfx/rgbpixel.h"
#include "ivideo/texture.h"
#include "iutil/objreg.h"
#include "ivaria/reporter.h"

SCF_IMPLEMENT_FACTORY (csSoftProcTexture3D)

SCF_IMPLEMENT_IBASE_EXT (csSoftProcTexture3D)
  SCF_IMPLEMENTS_EMBEDDED_INTERFACE (iSoftProcTexture)
SCF_IMPLEMENT_IBASE_EXT_END

SCF_IMPLEMENT_EMBEDDED_IBASE (csSoftProcTexture3D::eiSoftProcTexture)
  SCF_IMPLEMENTS_INTERFACE (iSoftProcTexture)
SCF_IMPLEMENT_EMBEDDED_IBASE_END

csSoftProcTexture3D::csSoftProcTexture3D (iBase *iParent)
  : csGraphics3DSoftwareCommon (iParent)
{
  SCF_CONSTRUCT_EMBEDDED_IBASE(scfiSoftProcTexture);
  is_for_procedural_textures = true;
  soft_tex_mm = NULL;
  parent_tex_mm = NULL;
  reprepare = false;
}

csSoftProcTexture3D::~csSoftProcTexture3D ()
{
  if (sharing)
  {
    // We are sharing the cache and texture manager
    tcache = NULL;
    texman = NULL;
  }
}

void csSoftProcTexture3D::Print (csRect *area)
{
  csGraphics3DSoftwareCommon::Print (area);

  if (reprepare)
    soft_tex_mm->ReprepareProcTexture ();

  // As we've printed something we need to uncache it so the effects will be
  // registered next frame.
  if (parent_tex_mm)
    main_tcache->uncache_texture (0, (iTextureHandle*)parent_tex_mm);
}

bool csSoftProcTexture3D::Prepare (csTextureManagerSoftware *main_texman,
  csTextureHandleSoftware *tex_mm, void *buffer,  uint8 *bitmap)
{
  csGraphics3DSoftwareCommon* parent_g3d = main_texman->G3D;
  iGraphics2D *g2d = parent_g3d->GetDriver2D ();
  object_reg = parent_g3d->object_reg;

  main_tcache = parent_g3d->tcache;
  parent_tex_mm = tex_mm;
  parent_tex_mm->GetMipMapDimensions (0, width, height);
  int flags = parent_tex_mm->GetFlags ();
  bool use8bit = (flags & CS_TEXTURE_PROC_ALONE_HINT) ==
                     CS_TEXTURE_PROC_ALONE_HINT;
  // Cases:
  // Current Display..Texture Manager..tex_mm flags.....Reprepare
  //       8bit         main             none               no
  //     15/16bit       main             none               yes
  //     15/16bit       proc   CS_TEXTURE_PROC_ALONE_HINT   no
  //       32bit        main             none               yes
  //       32bit        proc   CS_TEXTURE_PROC_ALONE_HINT   no
  //

  if (main_texman->pfmt.PixelBytes == 1)
  {
    sharing = true;
    reprepare = false;
    // Global palette
    G2D = g2d->CreateOffScreenCanvas (width, height, bitmap, use8bit,
		     &main_texman->pfmt,  &main_texman->cmap.palette[0], 256);
    if (!G2D) return false;

    SharedInitialize (parent_g3d);
    if (!Open () || !SharedOpen ())
      return false;

#ifdef CS_DEBUG
    printf ("8bit procedural texture\n");
#endif
  }
  else
  {
    if (!use8bit)
    {
      sharing = true;
      reprepare = true;
      soft_tex_mm = tex_mm;

      G2D = g2d->CreateOffScreenCanvas (width, height, buffer, false,
        &main_texman->pfmt, parent_tex_mm->GetColorMap (), 256);

      SharedInitialize (parent_g3d);
      if (!Open () || !SharedOpen ())
	return false;

#ifdef CS_DEBUG
      printf ("%sbit procedural texture\n",
        main_texman->pfmt.PixelBytes == 2 ? "16" : "32");
#endif
    }
    else
    {
      reprepare = false;
      G2D = g2d->CreateOffScreenCanvas (width, height, bitmap, true,
        &main_texman->pfmt, parent_tex_mm->GetColorMap (), 256);
      if (!G2D) return false;

      if (!main_texman->GetFirst8bitProcTexture ())
      {
        sharing = false;
        // We are the first procedural texture utilising a dedicated
        // procedural texture manager working in 8bit
        NewInitialize ();
        if (!Open () || !NewOpen ())
          return false;
        // Inform each texture manager aboout the other
        texman->SetMainTextureManager (main_texman);
        main_texman->SetProcTextureManager (texman);
        texman->ResetPalette ();
#ifdef CS_DEBUG
        printf ("Preparing dedicated procedural texture manager\n");
#endif
        // Initialize the texture manager
        int r,g,b;
        for (r = 0; r < 8; r++)
          for (g = 0; g < 8; g++)
            for (b = 0; b < 4; b++)
              texman->ReserveColor (r * 32, g * 32, b * 64);
        texman->PrepareTextures ();
      }
      else
      {
        sharing = true;
        SharedInitialize (main_texman->GetFirst8bitProcTexture ());
        if (!Open () || !SharedOpen ())
          return false;
      }

      if (buffer)
      {
        // In order to keep the palette synchronised
        iImage *im = (iImage*) new csImageMemory (width, height,
          (csRGBpixel *) buffer, false);
	  csTextureHandleSoftware *dummy =
          (csTextureHandleSoftware *)texman->RegisterTexture (im, CS_TEXTURE_2D | CS_TEXTURE_PROC);
        dummy->Prepare();
	dummy->DecRef();
        im->DecRef ();
      }

#ifdef CS_DEBUG
      printf ("%s/8bit procedural texture\n",
        main_texman->pfmt.PixelBytes == 2 ? "16" : "32");
#endif
    }
  }
  return true;
}

// The entry point for other than software graphics drivers..
iTextureHandle *csSoftProcTexture3D::CreateOffScreenRenderer
  (iGraphics3D *parent_g3d, iGraphics3D* g3d_partner, int width, int height,
   void *buffer, csPixelFormat *ipfmt, int flags)
{
  // Always in 32bit
  // Here we create additional images for this texture which are registered as
  // procedural textures with our own texture manager.  This way if the
  // procedural texture is written to with itself, this texture manager will
  // know about it and act accordingly.  This is done for both alone and
  // sharing procedural textures.  In the case of the alone procedural texture
  // it will never be utilised unless a sharing procedural texture is
  // introduced into the system in which case the previous alone procedural
  // textures are all converted to sharing ones.

  // If this is a sharing procedural texture then the texture needs repreparing
  // each update.
  if ((flags & CS_TEXTURE_PROC_ALONE_HINT) !=  CS_TEXTURE_PROC_ALONE_HINT)
    reprepare = true;

  G2D = parent_g3d->GetDriver2D ()->CreateOffScreenCanvas (
    width, height, buffer, false, ipfmt, NULL, 0);

  if (!G2D)
  {
    csReport (object_reg, CS_REPORTER_SEVERITY_ERROR,
      	"crystalspace.video.software.protex3d",
	"Error opening Graphics2D texture context.");
    return NULL;
  }

  if (g3d_partner)
  {
    sharing = true;
    partner = (csGraphics3DSoftwareCommon*)g3d_partner;
    SharedInitialize (partner);
    if (!Open () || !SharedOpen ())
      return NULL;
  }
  else
  {
    sharing = false;
    NewInitialize ();
    if (!Open () || !NewOpen ())
      return NULL;
  }

  // Register our own buffer as a procedural texture.
  // The texture handles GetProcTextureInterface () is never called so
  // no additional interfaces are created.

  // We set the 'destroy' parameter for csImageMemory to false as these buffers
  // are destroyed else where.
  iImage *tex_image = new csImageMemory (width, height,
					 (csRGBpixel*)buffer, false);

  soft_tex_mm = (csTextureHandleSoftware *)
        texman->RegisterTexture (tex_image, CS_TEXTURE_PROC | CS_TEXTURE_2D);
  soft_tex_mm->Prepare ();

  // Return the software texture managers handle for this procedural texture.
  return soft_tex_mm;
}
<|endoftext|>