<commit_before>/*
Resembla: Word-based Japanese similar sentence search library
https://github.com/tuem/resembla

Copyright 2017 Takashi Uemura

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef RESEMBLA_LETTER_WEIGHT_HPP
#define RESEMBLA_LETTER_WEIGHT_HPP

#include <string>
#include <fstream>
#include <exception>
#include <unordered_map>

#include "../string_util.hpp"

namespace resembla {

// simple weight function for sets of letters
template<typename string_type>
struct LetterWeight
{
    using value_type = typename string_type::value_type;

    LetterWeight(double base_weight, double delete_insert_ratio, const std::string& letter_weight_file_path):
        base_weight(base_weight), delete_insert_ratio(delete_insert_ratio)
    {
        if(letter_weight_file_path.empty()){
            return;
        }

        std::basic_ifstream<value_type> ifs(letter_weight_file_path);
        if(ifs.fail()){
            throw std::runtime_error("input file is not available: " + letter_weight_file_path);
        }

        while(ifs.good()){
            string_type line;
            std::getline(ifs, line);
            if(ifs.eof() || line.length() == 0){
                break;
            }

            auto columns = split(line, column_delimiter<value_type>());
            if(columns.size() < 2){
                throw std::runtime_error("invalid line in " + letter_weight_file_path + ": " + cast_string<std::string>(line));
            }

            auto letters= columns[0];
            auto weight = std::stod(columns[1]);
            for(size_t i = 0; i < letters.size(); ++i){
                letter_weights[letters[i]] = weight;
            }
        }
    }

    double operator()(const value_type c, bool is_original = false, size_t total = -1, size_t position = -1) const
    {
        (void)total;
        (void)position;

        double w = base_weight;
        if(is_original){
            w *= delete_insert_ratio;
        }

        auto p = letter_weights.find(c);
        if(p != std::end(letter_weights)){
            w *= p->second;
        }

        return w;
    }

protected:
    double base_weight;
    double delete_insert_ratio;

    std::unordered_map<value_type, double> letter_weights;
};

}
#endif
<commit_msg>use csv_reader in letter_weight<commit_after>/*
Resembla: Word-based Japanese similar sentence search library
https://github.com/tuem/resembla

Copyright 2017 Takashi Uemura

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef RESEMBLA_LETTER_WEIGHT_HPP
#define RESEMBLA_LETTER_WEIGHT_HPP

#include <string>
#include <exception>
#include <unordered_map>

#include "../string_util.hpp"
#include "../csv_reader.hpp"

namespace resembla {

// simple weight function for sets of letters
template<typename string_type>
struct LetterWeight
{
    using value_type = typename string_type::value_type;

    LetterWeight(double base_weight, double delete_insert_ratio, const std::string& letter_weight_file_path):
        base_weight(base_weight), delete_insert_ratio(delete_insert_ratio)
    {
        if(letter_weight_file_path.empty()){
            return;
        }

        for(const auto& columns: CsvReader<>(letter_weight_file_path, 2)){
            auto letters = cast_string<string_type>(columns[0]);
            auto weight = std::stod(columns[1]);

            for(auto c: letters){
                letter_weights[c] = weight;
            }
        }
    }

    double operator()(const value_type c, bool is_original = false, size_t total = -1, size_t position = -1) const
    {
        (void)total;
        (void)position;

        double w = base_weight;
        if(is_original){
            w *= delete_insert_ratio;
        }

        auto p = letter_weights.find(c);
        if(p != std::end(letter_weights)){
            w *= p->second;
        }

        return w;
    }

protected:
    double base_weight;
    double delete_insert_ratio;

    std::unordered_map<value_type, double> letter_weights;
};

}
#endif
<|endoftext|>


<commit_before>#include <aikido/statespace/RealVectorStateSpace.hpp>
#include <aikido/statespace/SO2StateSpace.hpp>
#include <aikido/statespace/SO3StateSpace.hpp>
#include <aikido/statespace/SE2StateSpace.hpp>
#include <aikido/statespace/SE3StateSpace.hpp>
#include <aikido/statespace/CompoundStateSpace.hpp>
#include <aikido/statespace/StateSpace.hpp>
#include <dart/math/Geometry.h>

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <typeinfo>       // std::bad_cast

using namespace aikido::statespace;
using namespace std;

TEST(RealVectorStateSpace, Compose)
{
  RealVectorStateSpace rvss(3);

  RealVectorStateSpace::ScopedState s1 = rvss.createState();
  s1.getValue() = Eigen::Vector3d(1, 2, 3);

  RealVectorStateSpace::ScopedState s2 = rvss.createState();
  s2.getValue() = Eigen::Vector3d(2, 3, 4);

  RealVectorStateSpace::ScopedState out = rvss.createState();
  rvss.compose(*s1, *s2, *out);

  EXPECT_TRUE(out.getValue().isApprox(Eigen::Vector3d(3, 5, 7)));

}

TEST(SO2StateSpace, Compose)
{
  SO2StateSpace::State s1(M_PI/4);
  SO2StateSpace::State s2(M_PI/2);
  SO2StateSpace::State out;
  SO2StateSpace::State expected(3.0/4.0*M_PI);

  SO2StateSpace so2;
  so2.compose(s1, s2, out);

  EXPECT_TRUE(out.getRotation().isApprox(expected.getRotation()));

}

TEST(SO3StateSpace, Compose)
{
  SO3StateSpace::State identity;
  EXPECT_TRUE(identity.getQuaternion().isApprox(
    Eigen::Quaterniond::Identity()));

  SO3StateSpace::State s2(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())));
  SO3StateSpace::State s3(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())));
  SO3StateSpace::State expected(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())));

  SO3StateSpace::State out;
  SO3StateSpace so3;
  so3.compose(s2, s3, out);

  EXPECT_TRUE(expected.getQuaternion().isApprox(out.getQuaternion()));
}

TEST(SE2StateSpace, Compose)
{
  SE2StateSpace space;

  SE2StateSpace::ScopedState identity(&space);
  EXPECT_TRUE(identity.getIsometry().isApprox(Eigen::Isometry2d::Identity()));

  Eigen::Isometry2d pose2 = Eigen::Isometry2d::Identity();
  pose2.rotate(Eigen::Rotation2Dd(M_PI_2));
  SE2StateSpace::ScopedState state2(&space);
  state2.setIsometry(pose2);

  Eigen::Isometry2d pose3 = Eigen::Isometry2d::Identity();
  pose3.rotate(Eigen::Rotation2Dd(M_PI_4));
  SE2StateSpace::ScopedState state3(&space);
  state3.setIsometry(pose3);

  Eigen::Isometry2d expected_pose = Eigen::Isometry2d::Identity();
  expected_pose.rotate(Eigen::Rotation2Dd(3. * M_PI_4));

  SE2StateSpace::ScopedState out(&space);
  space.compose(*state2, *state3, *out);

  EXPECT_TRUE(expected_pose.isApprox(out.getIsometry()));
}

TEST(SE3StateSpace, Compose)
{
  SE3StateSpace space;

  SE3StateSpace::ScopedState identity(&space);
  EXPECT_TRUE(identity.getIsometry().isApprox(Eigen::Isometry3d::Identity()));

  Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
  pose2.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX()));
  SE3StateSpace::ScopedState s2(&space);
  s2.setIsometry(pose2);

  Eigen::Isometry3d pose3 = Eigen::Isometry3d::Identity();
  pose3.rotate(Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitX()));
  SE3StateSpace::ScopedState s3(&space);
  s2.setIsometry(pose3);

  Eigen::Isometry3d expected = Eigen::Isometry3d::Identity();
  expected.rotate(Eigen::AngleAxisd(3. * M_PI_4, Eigen::Vector3d::UnitX()));

  SE3StateSpace::ScopedState out(&space);
  space.compose(*s2, *s3, *out);

  std::cout << "expected\n" << expected.matrix() << "\n"
            << "actual\n" << out.getIsometry().matrix() << "\n"
            << std::endl;

  EXPECT_TRUE(expected.isApprox(out.getIsometry()));
}

TEST(CompoundStateSpace, Compose)
{
  using Eigen::Vector2d;

  CompoundStateSpace space({
    std::make_shared<SO2StateSpace>(),
    std::make_shared<RealVectorStateSpace>(2)
  });

  // TODO: This syntax is _really_ bad.
  CompoundStateSpace::ScopedState s1 = space.createState();
  s1.getSubState<SO2StateSpace>(0).setAngle(M_PI_2);
  s1.getSubStateHandle<RealVectorStateSpace>(1).getValue() = Vector2d(3., 4.);

  CompoundStateSpace::ScopedState s2 = space.createState();
  space.getSubState<SO2StateSpace>(*s2, 0).setAngle(M_PI_2);
  space.getSubSpace<RealVectorStateSpace>(1).getValue(
    space.getSubState<RealVectorStateSpace>(*s2, 1)) = Vector2d(5., 10.);

  CompoundStateSpace::ScopedState out = space.createState();
  space.compose(*s1, *s2, *out);

  const double out1 = out.getSubState<SO2StateSpace>(0).getAngle();
  EXPECT_DOUBLE_EQ(M_PI, out1);

  const Vector2d out2 = out.getSubStateHandle<RealVectorStateSpace>(1).getValue();
  EXPECT_TRUE(out2.isApprox(Vector2d(8., 14.)));
}
<commit_msg>Tweaks to test.<commit_after>#include <aikido/statespace/RealVectorStateSpace.hpp>
#include <aikido/statespace/SO2StateSpace.hpp>
#include <aikido/statespace/SO3StateSpace.hpp>
#include <aikido/statespace/SE2StateSpace.hpp>
#include <aikido/statespace/SE3StateSpace.hpp>
#include <aikido/statespace/CompoundStateSpace.hpp>
#include <aikido/statespace/StateSpace.hpp>
#include <dart/math/Geometry.h>

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <typeinfo>       // std::bad_cast

using namespace aikido::statespace;
using namespace std;

TEST(RealVectorStateSpace, Compose)
{
  RealVectorStateSpace rvss(3);

  RealVectorStateSpace::ScopedState s1 = rvss.createState();
  s1.getValue() = Eigen::Vector3d(1, 2, 3);

  RealVectorStateSpace::ScopedState s2 = rvss.createState();
  s2.getValue() = Eigen::Vector3d(2, 3, 4);

  RealVectorStateSpace::ScopedState out = rvss.createState();
  rvss.compose(*s1, *s2, *out);

  EXPECT_TRUE(out.getValue().isApprox(Eigen::Vector3d(3, 5, 7)));

}

TEST(SO2StateSpace, Compose)
{
  SO2StateSpace::State s1(M_PI/4);
  SO2StateSpace::State s2(M_PI/2);
  SO2StateSpace::State out;
  SO2StateSpace::State expected(3.0/4.0*M_PI);

  SO2StateSpace so2;
  so2.compose(s1, s2, out);

  EXPECT_TRUE(out.getRotation().isApprox(expected.getRotation()));

}

TEST(SO3StateSpace, Compose)
{
  SO3StateSpace::State identity;
  EXPECT_TRUE(identity.getQuaternion().isApprox(
    Eigen::Quaterniond::Identity()));

  SO3StateSpace::State s2(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())));
  SO3StateSpace::State s3(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())));
  SO3StateSpace::State expected(Eigen::Quaterniond(
    Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())));

  SO3StateSpace::State out;
  SO3StateSpace so3;
  so3.compose(s2, s3, out);

  EXPECT_TRUE(expected.getQuaternion().isApprox(out.getQuaternion()));
}

TEST(SE2StateSpace, Compose)
{
  SE2StateSpace space;

  SE2StateSpace::ScopedState identity(&space);
  EXPECT_TRUE(identity.getIsometry().isApprox(Eigen::Isometry2d::Identity()));

  Eigen::Isometry2d pose2 = Eigen::Isometry2d::Identity();
  pose2.rotate(Eigen::Rotation2Dd(M_PI_2));
  SE2StateSpace::ScopedState state2(&space);
  state2.setIsometry(pose2);

  Eigen::Isometry2d pose3 = Eigen::Isometry2d::Identity();
  pose3.rotate(Eigen::Rotation2Dd(M_PI_4));
  SE2StateSpace::ScopedState state3(&space);
  state3.setIsometry(pose3);

  Eigen::Isometry2d expected_pose = Eigen::Isometry2d::Identity();
  expected_pose.rotate(Eigen::Rotation2Dd(3. * M_PI_4));

  SE2StateSpace::ScopedState out(&space);
  space.compose(*state2, *state3, *out);

  EXPECT_TRUE(expected_pose.isApprox(out.getIsometry()));
}

TEST(SE3StateSpace, Compose)
{
  SE3StateSpace space;

  SE3StateSpace::ScopedState identity(&space);
  EXPECT_TRUE(identity.getIsometry().isApprox(Eigen::Isometry3d::Identity()));

  Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
  pose2.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX()));
  SE3StateSpace::ScopedState s2(&space);
  s2.setIsometry(pose2);

  Eigen::Isometry3d pose3 = Eigen::Isometry3d::Identity();
  pose3.rotate(Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitX()));
  SE3StateSpace::ScopedState s3(&space);
  s2.setIsometry(pose3);

  Eigen::Isometry3d expected = pose2 * pose3;

  SE3StateSpace::ScopedState out(&space);
  space.compose(*s2, *s3, *out);

  std::cout << "expected\n" << expected.matrix() << "\n"
            << "actual\n" << out.getIsometry().matrix() << "\n"
            << std::endl;

  EXPECT_TRUE(expected.isApprox(out.getIsometry()));
}

TEST(CompoundStateSpace, Compose)
{
  using Eigen::Vector2d;

  CompoundStateSpace space({
    std::make_shared<SO2StateSpace>(),
    std::make_shared<RealVectorStateSpace>(2)
  });

  // TODO: This syntax is _really_ bad.
  CompoundStateSpace::ScopedState s1 = space.createState();
  s1.getSubState<SO2StateSpace>(0).setAngle(M_PI_2);
  s1.getSubStateHandle<RealVectorStateSpace>(1).getValue() = Vector2d(3., 4.);

  CompoundStateSpace::ScopedState s2 = space.createState();
  space.getSubState<SO2StateSpace>(*s2, 0).setAngle(M_PI_2);
  space.getSubSpace<RealVectorStateSpace>(1).getValue(
    space.getSubState<RealVectorStateSpace>(*s2, 1)) = Vector2d(5., 10.);

  CompoundStateSpace::ScopedState out = space.createState();
  space.compose(*s1, *s2, *out);

  const double out1 = out.getSubState<SO2StateSpace>(0).getAngle();
  EXPECT_DOUBLE_EQ(M_PI, out1);

  const Vector2d out2 = out.getSubStateHandle<RealVectorStateSpace>(1).getValue();
  EXPECT_TRUE(out2.isApprox(Vector2d(8., 14.)));
}
<|endoftext|>


<commit_before>#include "mem/cache/tags/utillru.hh"
#include "base/intmath.hh"
#include "debug/CacheRepl.hh"
#include "mem/cache/tags/cacheset.hh"
#include "mem/cache/tags/lru.hh"
#include "mem/cache/base.hh"
#include "sim/core.hh"
#include "mem/cache/blk.hh"
#include <typeinfo>

UTILLRU::UTILLRU( unsigned _numSets,
        unsigned _blkSize,
        unsigned _assoc,
        unsigned _hit_latency,
		unsigned _L_assoc)
    : LRU(_numSets, _blkSize, _assoc, _hit_latency )
{
	L_assoc = _L_assoc;
	H_assoc = _assoc - L_assoc;
	assoc = _assoc;
	// umon counters
	umon_counters = new unsigned*[2];
	for (unsigned i = 0; i < 2; i++)
		umon_counters[i] = new unsigned[assoc];

	for (unsigned i = 0; i < 2; i++)
		for (unsigned j = 0; j < assoc; j++)
			umon_counters[i][j] = 0;

	init_sets();
}

void UTILLRU::reset_umon()
{
	for (unsigned i = 0; i < 2; i++)
		for (unsigned j = 0; j < assoc; j++)
			umon_counters[i][j] = umon_counters[i][j]/2;
}

unsigned UTILLRU::curr_L_assoc()
{
	return L_assoc;
}

unsigned UTILLRU::lookup_umon(int index, uint64_t tid)
{
	return umon_counters[tid][index];
}

int
UTILLRU::assoc_of_tc( int tcid ){
    if (tcid == 0) return L_assoc;
	else return H_assoc;
}

CacheSet
UTILLRU::get_set( int setnum, uint64_t tid, Addr addr ){
    return sets[setnum];
}

void
UTILLRU::init_sets(){
    sets = new CacheSet[numSets];

    numBlocks = numSets * assoc;
    blks = new BlkType[numBlocks];
    dataBlks = new uint8_t[numBlocks * blkSize];

    unsigned blkIndex = 0;
    for( unsigned i = 0; i< numSets; i++ ){
        sets[i].assoc = assoc;
        sets[i].blks = new BlkType*[assoc];
        for( unsigned j = 0; j<assoc; j++ ){
            BlkType *blk = &blks[blkIndex];
            blk->data = &dataBlks[blkSize*blkIndex];
            ++blkIndex;

            blk->status = 0;
            blk->tag = j;
            blk->whenReady = 0;
            blk->isTouched = false;
            blk->size = blkSize;
            blk->set = i;
			// initialize the threadID
			blk->threadID = 0;
            sets[i].blks[j]=blk;
        }
    }

	// shadow tags for monitoring
	umon_sets = new CacheSet*[2];
	for( int i=0; i< 2; i++ ){ umon_sets[i] = new CacheSet[numSets]; }
	umon_blks = new BlkType*[2];
	for( int i=0; i< 2; i++ ){ umon_blks[i] = new BlkType[numBlocks]; }

	for( unsigned tc=0; tc<2; tc++ ){
		blkIndex = 0;
		for( unsigned i = 0; i < numSets; i++){
			umon_sets[tc][i].assoc = assoc;
			umon_sets[tc][i].blks = new BlkType*[assoc];
	        for( unsigned j = 0; j<assoc; j++ ){
	            BlkType *blk = &umon_blks[tc][blkIndex];
	            blk->data = 0;
	            ++blkIndex;

	            blk->status = 0;
	            blk->tag = j;
	            blk->whenReady = 0;
	            blk->isTouched = false;
	            blk->size = blkSize;
	            blk->set = i;
				// initialize the threadID
				blk->threadID = tc;
	            umon_sets[tc][i].blks[j]=blk;
	        }
		}
	}
}

// increase the size of Low partition
unsigned
UTILLRU::inc_size(){
	if(H_assoc == 1) return 0;

	L_assoc += 1;
	H_assoc -= 1;

	fprintf(stderr, "L_assoc = %2d H_assoc = %2d\n", L_assoc, H_assoc);

	return numSets;
}

unsigned
UTILLRU::dec_size(){
	if(L_assoc == 1) return 0;

	L_assoc -= 1;
	H_assoc += 1;

	fprintf(stderr, "L_assoc = %2d H_assoc = %2d\n", L_assoc, H_assoc);

	return numSets;
}

UTILLRU::BlkType*
UTILLRU::accessBlock(Addr addr, int &lat, int master_id, uint64_t tid)
{
    Addr tag = extractTag(addr);
    unsigned set = extractSet(addr);
    // BlkType *blk = sets[set].findBlk(tag);
    BlkType *blk = get_set(set,tid,addr).findBlk(tag);
    lat = hitLatency;
    if (blk != 0 ) {
        // move this block to head of the MRU list
        get_set(set,tid,addr).moveToHead(blk);
        DPRINTF(CacheRepl, "set %x: moving blk %x to MRU\n",
                set, regenerateBlkAddr(tag, set));
        if (blk->whenReady > curTick()
            && blk->whenReady - curTick() > hitLatency) {
            lat = blk->whenReady - curTick();
        }
        blk->refCount += 1;
		// set the used bit
		blk->isTouched = 1;
    }

	BlkType *umon_blk = umon_sets[tid][set].findBlk(tag);
	unsigned index = umon_sets[tid][set].findBlkIndex(tag);
	// cache hit on umon tags
	if (umon_blk != 0) {
		umon_counters[index]++;
		umon_sets[tid][set].moveToHead(umon_blk);
	}
	// cache miss on umon tags
	else {
		umon_sets[tid][set].blks[assoc-1]->tag = tag;
		umon_sets[tid][set].blks[assoc-1]->status = BlkValid | BlkReadable;
		umon_sets[tid][set].moveToHead(umon_sets[tid][set].blks[assoc-1]);
	}

    return blk;
}

UTILLRU::BlkType*
UTILLRU::findBlock(Addr addr, uint64_t tid)
{
    Addr tag = extractTag(addr);
    unsigned set = extractSet(addr);
    BlkType *blk = get_set(set,tid,addr).findBlk(tag);
    return blk;
}

UTILLRU::BlkType*
UTILLRU::findVictim(Addr addr, PacketList &writebacks, uint64_t tid)
{
    unsigned set = extractSet(addr);
    // grab a replacement candidate
    BlkType *blk = get_set(set,tid,addr).findVictim(tid, L_assoc);

    if (blk->isValid()) {
        DPRINTF(CacheRepl, "set %x: selecting blk %x for replacement\n",
                set, regenerateBlkAddr(blk->tag, set));
    }
    return blk;
}

void
UTILLRU::invalidateBlk(BlkType *blk, uint64_t tid)
{
    if (blk) {
        if (blk->isValid()) {
            tagsInUse--;
            assert(blk->srcMasterId < cache->system->maxMasters());
            occupancies[blk->srcMasterId]--;
            blk->srcMasterId = Request::invldMasterId;
            unsigned set = blk->set;
			get_set(set,tid,blk->set).moveToTail(blk);
        }
        blk->status = 0;
        blk->isTouched = false;
        blk->clearLoadLocks();

        // should be evicted before valid blocks
        //unsigned set = blk->set;
        //sets[set].moveToTail(blk);
    }

	BlkType *umon_blk = umon_sets[tid][blk->set].findBlk(blk->tag);
	if (umon_blk != 0) {
		if (umon_blk->isValid()) {
			umon_sets[tid][blk->set].moveToTail(umon_blk);
		}
		umon_blk->status = 0;
		umon_blk->isTouched = false;
	}
}<commit_msg>fixed a segmentation fault<commit_after>#include "mem/cache/tags/utillru.hh"
#include "base/intmath.hh"
#include "debug/CacheRepl.hh"
#include "mem/cache/tags/cacheset.hh"
#include "mem/cache/tags/lru.hh"
#include "mem/cache/base.hh"
#include "sim/core.hh"
#include "mem/cache/blk.hh"
#include <typeinfo>

UTILLRU::UTILLRU( unsigned _numSets,
        unsigned _blkSize,
        unsigned _assoc,
        unsigned _hit_latency,
		unsigned _L_assoc)
    : LRU(_numSets, _blkSize, _assoc, _hit_latency )
{
	L_assoc = _L_assoc;
	H_assoc = _assoc - L_assoc;
	assoc = _assoc;
	// umon counters
	umon_counters = new unsigned*[2];
	for (unsigned i = 0; i < 2; i++)
		umon_counters[i] = new unsigned[assoc];

	for (unsigned i = 0; i < 2; i++)
		for (unsigned j = 0; j < assoc; j++)
			umon_counters[i][j] = 0;

	init_sets();
}

void UTILLRU::reset_umon()
{
	for (unsigned i = 0; i < 2; i++)
		for (unsigned j = 0; j < assoc; j++)
			umon_counters[i][j] = umon_counters[i][j]/2;
}

unsigned UTILLRU::curr_L_assoc()
{
	return L_assoc;
}

unsigned UTILLRU::lookup_umon(int index, uint64_t tid)
{
	return umon_counters[tid][index];
}

int
UTILLRU::assoc_of_tc( int tcid ){
    if (tcid == 0) return L_assoc;
	else return H_assoc;
}

CacheSet
UTILLRU::get_set( int setnum, uint64_t tid, Addr addr ){
    return sets[setnum];
}

void
UTILLRU::init_sets(){
    sets = new CacheSet[numSets];

    numBlocks = numSets * assoc;
    blks = new BlkType[numBlocks];
    dataBlks = new uint8_t[numBlocks * blkSize];

    unsigned blkIndex = 0;
    for( unsigned i = 0; i< numSets; i++ ){
        sets[i].assoc = assoc;
        sets[i].blks = new BlkType*[assoc];
        for( unsigned j = 0; j<assoc; j++ ){
            BlkType *blk = &blks[blkIndex];
            blk->data = &dataBlks[blkSize*blkIndex];
            ++blkIndex;

            blk->status = 0;
            blk->tag = j;
            blk->whenReady = 0;
            blk->isTouched = false;
            blk->size = blkSize;
            blk->set = i;
			// initialize the threadID
			blk->threadID = 0;
            sets[i].blks[j]=blk;
        }
    }

	// shadow tags for monitoring
	umon_sets = new CacheSet*[2];
	for( int i=0; i< 2; i++ ){ umon_sets[i] = new CacheSet[numSets]; }
	umon_blks = new BlkType*[2];
	for( int i=0; i< 2; i++ ){ umon_blks[i] = new BlkType[numBlocks]; }

	for( unsigned tc=0; tc<2; tc++ ){
		blkIndex = 0;
		for( unsigned i = 0; i < numSets; i++){
			umon_sets[tc][i].assoc = assoc;
			umon_sets[tc][i].blks = new BlkType*[assoc];
	        for( unsigned j = 0; j<assoc; j++ ){
	            BlkType *blk = &umon_blks[tc][blkIndex];
	            blk->data = 0;
	            ++blkIndex;

	            blk->status = 0;
	            blk->tag = j;
	            blk->whenReady = 0;
	            blk->isTouched = false;
	            blk->size = blkSize;
	            blk->set = i;
				// initialize the threadID
				blk->threadID = tc;
	            umon_sets[tc][i].blks[j]=blk;
	        }
		}
	}
}

// increase the size of Low partition
unsigned
UTILLRU::inc_size(){
	if(H_assoc == 1) return 0;

	L_assoc += 1;
	H_assoc -= 1;

	fprintf(stderr, "L_assoc = %2d H_assoc = %2d\n", L_assoc, H_assoc);

	return numSets;
}

unsigned
UTILLRU::dec_size(){
	if(L_assoc == 1) return 0;

	L_assoc -= 1;
	H_assoc += 1;

	fprintf(stderr, "L_assoc = %2d H_assoc = %2d\n", L_assoc, H_assoc);

	return numSets;
}

UTILLRU::BlkType*
UTILLRU::accessBlock(Addr addr, int &lat, int master_id, uint64_t tid)
{
    Addr tag = extractTag(addr);
    unsigned set = extractSet(addr);
    // BlkType *blk = sets[set].findBlk(tag);
    BlkType *blk = get_set(set,tid,addr).findBlk(tag);
    lat = hitLatency;
    if (blk != 0 ) {
        // move this block to head of the MRU list
        get_set(set,tid,addr).moveToHead(blk);
        DPRINTF(CacheRepl, "set %x: moving blk %x to MRU\n",
                set, regenerateBlkAddr(tag, set));
        if (blk->whenReady > curTick()
            && blk->whenReady - curTick() > hitLatency) {
            lat = blk->whenReady - curTick();
        }
        blk->refCount += 1;
		// set the used bit
		blk->isTouched = 1;
    }

	BlkType *umon_blk = umon_sets[tid][set].findBlk(tag);
	unsigned index = umon_sets[tid][set].findBlkIndex(tag);
	// cache hit on umon tags
	if (umon_blk != 0) {
		umon_counters[tid][index]++;
		umon_sets[tid][set].moveToHead(umon_blk);
	}
	// cache miss on umon tags
	else {
		umon_sets[tid][set].blks[assoc-1]->tag = tag;
		umon_sets[tid][set].blks[assoc-1]->status = BlkValid | BlkReadable;
		umon_sets[tid][set].moveToHead(umon_sets[tid][set].blks[assoc-1]);
	}

    return blk;
}

UTILLRU::BlkType*
UTILLRU::findBlock(Addr addr, uint64_t tid)
{
    Addr tag = extractTag(addr);
    unsigned set = extractSet(addr);
    BlkType *blk = get_set(set,tid,addr).findBlk(tag);
    return blk;
}

UTILLRU::BlkType*
UTILLRU::findVictim(Addr addr, PacketList &writebacks, uint64_t tid)
{
    unsigned set = extractSet(addr);
    // grab a replacement candidate
    BlkType *blk = get_set(set,tid,addr).findVictim(tid, L_assoc);

    if (blk->isValid()) {
        DPRINTF(CacheRepl, "set %x: selecting blk %x for replacement\n",
                set, regenerateBlkAddr(blk->tag, set));
    }
    return blk;
}

void
UTILLRU::invalidateBlk(BlkType *blk, uint64_t tid)
{
    if (blk) {
        if (blk->isValid()) {
            tagsInUse--;
            assert(blk->srcMasterId < cache->system->maxMasters());
            occupancies[blk->srcMasterId]--;
            blk->srcMasterId = Request::invldMasterId;
            unsigned set = blk->set;
			get_set(set,tid,blk->set).moveToTail(blk);
        }
        blk->status = 0;
        blk->isTouched = false;
        blk->clearLoadLocks();

        // should be evicted before valid blocks
        //unsigned set = blk->set;
        //sets[set].moveToTail(blk);
    }

	BlkType *umon_blk = umon_sets[tid][blk->set].findBlk(blk->tag);
	if (umon_blk != 0) {
		if (umon_blk->isValid()) {
			umon_sets[tid][blk->set].moveToTail(umon_blk);
		}
		umon_blk->status = 0;
		umon_blk->isTouched = false;
	}
}<|endoftext|>


<commit_before>#ifndef VIENNAMETA_TYPELIST_HPP
#define VIENNAMETA_TYPELIST_HPP

#include "utils.hpp"
#include "typelist_macros.hpp"

namespace viennameta
{

    template <class head_, class tail_>
    struct typelist_t
    {
       typedef head_ head;
       typedef tail_ tail;
    };

    namespace typelist
    {
        namespace result_of
        {

            // size of a typelist
            template <typename typelist> struct size;

            template <> struct size<null_type>
            {
                enum { value = 0 };
            };

            template <typename head, typename tail>
            struct size< typelist_t<head, tail> >
            {
                enum { value = 1 + size<tail>::value };
            };

            // index access
            template <typename typelist, unsigned int index> struct at
            {
                typedef out_of_range type;
            };

            template <typename head, typename tail>
            struct at<typelist_t<head, tail>, 0>
            {
                typedef head type;
            };

            template <typename head, typename tail, unsigned int i>
            struct at<typelist_t<head, tail>, i>
            {
                typedef typename at<tail, i - 1>::type type;
            };

            // find first type
            template <typename typelist, typename to_find> struct find;

            template <typename to_find>
            struct find<null_type, to_find>
            {
                enum { value = -1 };
            };

            template <typename to_find, typename tail>
            struct find<typelist_t<to_find, tail>, to_find>
            {
                enum { value = 0 };
            };

            template <typename head, typename tail, typename to_find>
            struct find<typelist_t<head, tail>, to_find>
            {
            private:
                enum { temp = find<tail, to_find>::value };
            public:
                enum { value = (temp == -1 ? -1 : 1 + temp) };
            };

            // push back a single type
            template <typename typelist, typename to_add> struct push_back;

            template <>
            struct push_back<null_type, null_type>
            {
                typedef null_type type;
            };

            template <typename to_add>
            struct push_back<null_type, to_add>
            {
                typedef typelist_t<to_add, null_type> type;
            };

            template <typename head, typename tail, typename to_add>
            struct push_back<typelist_t<head, tail>, to_add>
            {
                typedef typelist_t<head, typename push_back<tail, to_add>::type> type;
            };

            // push back list
            template <typename typelist, typename typelist_to_add> struct push_back_list;

            template <>
            struct push_back_list<null_type, null_type>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct push_back_list<null_type, typelist_t<head, tail> >
            {
                typedef typelist_t<head, tail> type;
            };

            template <typename head, typename tail>
            struct push_back_list<typelist_t<head, tail>, null_type >
            {
                typedef typelist_t<head, tail> type;
            };

            template <typename head1, typename tail1, typename head2, typename tail2>
            struct push_back_list<typelist_t<head1, tail1>, typelist_t<head2, tail2> >
            {
                typedef typename push_back_list< typename push_back< typelist_t<head1, tail1>, head2>::type, tail2 >::type type;
            };

            // erase_at the type at index
            template <typename typelist, unsigned int index_to_erase> struct erase_at;

            template <unsigned int index_to_erase>
            struct erase_at<null_type, index_to_erase>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct erase_at< typelist_t<head, tail>, 0>
            {
                typedef tail type;
            };

            template <typename head, typename tail, unsigned int index_to_erase>
            struct erase_at< typelist_t<head, tail>, index_to_erase>
            {
                typedef typelist_t<head, typename erase_at<tail, index_to_erase-1>::type> type;
            };

            // erase the first type
            template <typename typelist, typename to_erase> struct erase;

            template <typename to_erase>
            struct erase<null_type, to_erase>
            {
                typedef null_type type;
            };

            template <typename to_erase, typename tail>
            struct erase< typelist_t<to_erase, tail>, to_erase>
            {
                typedef tail type;
            };

            template <typename head, typename tail, typename to_erase>
            struct erase< typelist_t<head, tail>, to_erase>
            {
                typedef typelist_t<head, typename erase<tail, to_erase>::type> type;
            };

            // erase all types
            template <typename typelist, typename to_erase>
            struct erase_all;

            template <typename to_erase>
            struct erase_all<null_type, to_erase>
            {
                typedef null_type type;
            };

            template <typename to_erase, typename tail>
            struct erase_all< typelist_t<to_erase, tail>, to_erase>
            {
                // Go all the way down the list removing the type
                typedef typename erase_all<tail, to_erase>::type type;
            };
            template <typename head, typename tail, typename to_erase>

            struct erase_all< typelist_t<head, tail>, to_erase>
            {
                // Go all the way down the list removing the type
                typedef typelist_t<head, typename erase_all<tail, to_erase>::type> type;
            };

            // no duplicate types
            template <typename typelist> struct no_duplicates;

            template <> struct
            no_duplicates<null_type>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct no_duplicates< typelist_t<head, tail> >
            {
            private:
                typedef typename no_duplicates<tail>::type L1;
                typedef typename erase<L1, head>::type L2;
            public:
                typedef typelist_t<head, L2> type;
            };

            // replace the type at index by another type
            template <typename typelist, unsigned int index_to_replace, typename replaced> struct replace_at;

            template <unsigned int index_to_replace, typename replaced>
            struct replace_at<null_type, index_to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename head, typename tail, typename replaced>
            struct replace_at<typelist_t<head, tail>, 0, replaced>
            {
                typedef typelist_t<replaced, tail> type;
            };

            template <typename head, typename tail, unsigned int index_to_replace, typename replaced>
            struct replace_at<typelist_t<head, tail>, index_to_replace, replaced>
            {
                typedef typelist_t<head, typename replace_at<tail, index_to_replace-1, replaced>::type> type;
            };

            // replace the first type by another type
            template <typename typelist, typename to_replace, typename replaced> struct replace;

            template <typename to_replace, typename replaced>
            struct replace<null_type, to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename to_replace, typename tail, typename replaced>
            struct replace<typelist_t<to_replace, tail>, to_replace, replaced>
            {
                typedef typelist_t<replaced, tail> type;
            };

            template <typename head, typename tail, typename to_replace, typename replaced>
            struct replace<typelist_t<head, tail>, to_replace, replaced>
            {
                typedef typelist_t<head, typename replace<tail, to_replace, replaced>::type> type;
            };

            // replace all types by another type
            template <typename typelist, typename to_replace, typename replaced> struct replace_all;

            template <typename to_replace, typename replaced>
            struct replace_all<null_type, to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename to_replace, typename tail, typename replaced>
            struct replace_all<typelist_t<to_replace, tail>, to_replace, replaced>
            {
                typedef typelist_t<replaced, typename replace_all<tail, to_replace, replaced>::type> type;
            };

            template <typename head, typename tail, typename to_replace, typename replaced>
            struct replace_all<typelist_t<head, tail>, to_replace, replaced>
            {
                typedef typelist_t<head, typename replace_all<tail, to_replace, replaced>::type> type;
            };
        }
    }
}

#endif // end file guardian

<commit_msg>renamed find to index_of<commit_after>#ifndef VIENNAMETA_TYPELIST_HPP
#define VIENNAMETA_TYPELIST_HPP

#include "utils.hpp"
#include "typelist_macros.hpp"

namespace viennameta
{

    template <class head_, class tail_>
    struct typelist_t
    {
       typedef head_ head;
       typedef tail_ tail;
    };

    namespace typelist
    {
        namespace result_of
        {

            // size of a typelist
            template <typename typelist> struct size;

            template <> struct size<null_type>
            {
                enum { value = 0 };
            };

            template <typename head, typename tail>
            struct size< typelist_t<head, tail> >
            {
                enum { value = 1 + size<tail>::value };
            };

            // index access
            template <typename typelist, unsigned int index> struct at
            {
                typedef out_of_range type;
            };

            template <typename head, typename tail>
            struct at<typelist_t<head, tail>, 0>
            {
                typedef head type;
            };

            template <typename head, typename tail, unsigned int i>
            struct at<typelist_t<head, tail>, i>
            {
                typedef typename at<tail, i - 1>::type type;
            };

            // returns the index of the first type in the typelist
            template <typename typelist, typename to_find> struct index_of;

            template <typename to_find>
            struct index_of<null_type, to_find>
            {
                enum { value = -1 };
            };

            template <typename to_find, typename tail>
            struct index_of<typelist_t<to_find, tail>, to_find>
            {
                enum { value = 0 };
            };

            template <typename head, typename tail, typename to_find>
            struct index_of<typelist_t<head, tail>, to_find>
            {
            private:
                enum { temp = index_of<tail, to_find>::value };
            public:
                enum { value = (temp == -1 ? -1 : 1 + temp) };
            };

            // push back a single type
            template <typename typelist, typename to_add> struct push_back;

            template <>
            struct push_back<null_type, null_type>
            {
                typedef null_type type;
            };

            template <typename to_add>
            struct push_back<null_type, to_add>
            {
                typedef typelist_t<to_add, null_type> type;
            };

            template <typename head, typename tail, typename to_add>
            struct push_back<typelist_t<head, tail>, to_add>
            {
                typedef typelist_t<head, typename push_back<tail, to_add>::type> type;
            };

            // push back list
            template <typename typelist, typename typelist_to_add> struct push_back_list;

            template <>
            struct push_back_list<null_type, null_type>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct push_back_list<null_type, typelist_t<head, tail> >
            {
                typedef typelist_t<head, tail> type;
            };

            template <typename head, typename tail>
            struct push_back_list<typelist_t<head, tail>, null_type >
            {
                typedef typelist_t<head, tail> type;
            };

            template <typename head1, typename tail1, typename head2, typename tail2>
            struct push_back_list<typelist_t<head1, tail1>, typelist_t<head2, tail2> >
            {
                typedef typename push_back_list< typename push_back< typelist_t<head1, tail1>, head2>::type, tail2 >::type type;
            };

            // erase_at the type at index
            template <typename typelist, unsigned int index_to_erase> struct erase_at;

            template <unsigned int index_to_erase>
            struct erase_at<null_type, index_to_erase>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct erase_at< typelist_t<head, tail>, 0>
            {
                typedef tail type;
            };

            template <typename head, typename tail, unsigned int index_to_erase>
            struct erase_at< typelist_t<head, tail>, index_to_erase>
            {
                typedef typelist_t<head, typename erase_at<tail, index_to_erase-1>::type> type;
            };

            // erase the first type
            template <typename typelist, typename to_erase> struct erase;

            template <typename to_erase>
            struct erase<null_type, to_erase>
            {
                typedef null_type type;
            };

            template <typename to_erase, typename tail>
            struct erase< typelist_t<to_erase, tail>, to_erase>
            {
                typedef tail type;
            };

            template <typename head, typename tail, typename to_erase>
            struct erase< typelist_t<head, tail>, to_erase>
            {
                typedef typelist_t<head, typename erase<tail, to_erase>::type> type;
            };

            // erase all types
            template <typename typelist, typename to_erase>
            struct erase_all;

            template <typename to_erase>
            struct erase_all<null_type, to_erase>
            {
                typedef null_type type;
            };

            template <typename to_erase, typename tail>
            struct erase_all< typelist_t<to_erase, tail>, to_erase>
            {
                // Go all the way down the list removing the type
                typedef typename erase_all<tail, to_erase>::type type;
            };
            template <typename head, typename tail, typename to_erase>

            struct erase_all< typelist_t<head, tail>, to_erase>
            {
                // Go all the way down the list removing the type
                typedef typelist_t<head, typename erase_all<tail, to_erase>::type> type;
            };

            // no duplicate types
            template <typename typelist> struct no_duplicates;

            template <> struct
            no_duplicates<null_type>
            {
                typedef null_type type;
            };

            template <typename head, typename tail>
            struct no_duplicates< typelist_t<head, tail> >
            {
            private:
                typedef typename no_duplicates<tail>::type L1;
                typedef typename erase<L1, head>::type L2;
            public:
                typedef typelist_t<head, L2> type;
            };

            // replace the type at index by another type
            template <typename typelist, unsigned int index_to_replace, typename replaced> struct replace_at;

            template <unsigned int index_to_replace, typename replaced>
            struct replace_at<null_type, index_to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename head, typename tail, typename replaced>
            struct replace_at<typelist_t<head, tail>, 0, replaced>
            {
                typedef typelist_t<replaced, tail> type;
            };

            template <typename head, typename tail, unsigned int index_to_replace, typename replaced>
            struct replace_at<typelist_t<head, tail>, index_to_replace, replaced>
            {
                typedef typelist_t<head, typename replace_at<tail, index_to_replace-1, replaced>::type> type;
            };

            // replace the first type by another type
            template <typename typelist, typename to_replace, typename replaced> struct replace;

            template <typename to_replace, typename replaced>
            struct replace<null_type, to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename to_replace, typename tail, typename replaced>
            struct replace<typelist_t<to_replace, tail>, to_replace, replaced>
            {
                typedef typelist_t<replaced, tail> type;
            };

            template <typename head, typename tail, typename to_replace, typename replaced>
            struct replace<typelist_t<head, tail>, to_replace, replaced>
            {
                typedef typelist_t<head, typename replace<tail, to_replace, replaced>::type> type;
            };

            // replace all types by another type
            template <typename typelist, typename to_replace, typename replaced> struct replace_all;

            template <typename to_replace, typename replaced>
            struct replace_all<null_type, to_replace, replaced>
            {
                typedef null_type type;
            };

            template <typename to_replace, typename tail, typename replaced>
            struct replace_all<typelist_t<to_replace, tail>, to_replace, replaced>
            {
                typedef typelist_t<replaced, typename replace_all<tail, to_replace, replaced>::type> type;
            };

            template <typename head, typename tail, typename to_replace, typename replaced>
            struct replace_all<typelist_t<head, tail>, to_replace, replaced>
            {
                typedef typelist_t<head, typename replace_all<tail, to_replace, replaced>::type> type;
            };
        }
    }
}

#endif // end file guardian

<|endoftext|>


<commit_before>#ifndef VIENNAGRID_STORAGE_RANGE_HPP
#define VIENNAGRID_STORAGE_RANGE_HPP

namespace viennagrid
{

    namespace storage
    {

        template<typename container_type>
        class container_range_wrapper
        {
            friend class container_range_wrapper<const container_type>;

        public:

            container_range_wrapper(container_type & _container) : container(_container) {}

            typedef typename container_type::size_type size_type;
            typedef typename container_type::value_type value_type;

            typedef typename container_type::reference reference;
            typedef typename container_type::const_reference const_reference;

            typedef typename container_type::pointer pointer;
            typedef typename container_type::const_pointer const_pointer;

            typedef typename container_type::iterator iterator;
            typedef typename container_type::const_iterator const_iterator;

            typedef typename container_type::reverse_iterator reverse_iterator;
            typedef typename container_type::const_reverse_iterator const_reverse_iterator;

            iterator begin() { return container.begin(); }
            const_iterator begin() const { return container.begin(); }
            iterator end() { return container.end(); }
            const_iterator end() const { return container.end(); }

            reverse_iterator rbegin() { return container.rbegin(); }
            const_reverse_iterator rbegin() const { return container.rbegin(); }
            reverse_iterator rend() { return container.rend(); }
            const_reverse_iterator rend() const { return container.rend(); }

            reference front() { return container.front(); }
            const_reference front() const { return container.front(); }
            reference back() { return container.back(); }
            const_reference back() const { return container.back(); }

            reference operator[] (size_type index) { return container[index]; }
            const_reference operator[] (size_type index) const { return container[index]; }

            bool empty() const { return container.empty(); }
            size_type size() const { return container.size(); }

        private:

            container_type & container;
        };

        template<typename container_type>
        class container_range_wrapper<const container_type>
        {
        public:

            container_range_wrapper(const container_type & _container) : container(_container) {}
            container_range_wrapper(const container_range_wrapper<container_type> & rhs) : container(rhs.container) {}

            typedef typename container_type::size_type size_type;
            typedef typename container_type::value_type value_type;

            typedef typename container_type::const_reference reference;
            typedef typename container_type::const_reference const_reference;

            typedef typename container_type::const_pointer pointer;
            typedef typename container_type::const_pointer const_pointer;

            typedef typename container_type::const_iterator iterator;
            typedef typename container_type::const_iterator const_iterator;

            typedef typename container_type::const_reverse_iterator reverse_iterator;
            typedef typename container_type::const_reverse_iterator const_reverse_iterator;

            iterator begin() { return container.begin(); }
            const_iterator begin() const { return container.begin(); }
            iterator end() { return container.end(); }
            const_iterator end() const { return container.end(); }

            reverse_iterator rbegin() { return container.rbegin(); }
            const_reverse_iterator rbegin() const { return container.rbegin(); }
            reverse_iterator rend() { return container.rend(); }
            const_reverse_iterator rend() const { return container.rend(); }

            reference front() { return container.front(); }
            const_reference front() const { return container.front(); }
            reference back() { return container.back(); }
            const_reference back() const { return container.back(); }

            reference operator[] (size_type index) { return container[index]; }
            const_reference operator[] (size_type index) const { return container[index]; }

            bool empty() const { return container.empty(); }
            size_type size() const { return container.size(); }

        private:

            const container_type & container;
        };

        template<typename iterator_type>
        class forward_iterator_range
        {
        public:

            forward_iterator_range(iterator_type _first, iterator_type _last) : first(_first), last(_last) {}

            typedef typename iterator_type::T value_type;

            typedef typename iterator_type::Reference reference;
            typedef const typename iterator_type::Reference const_reference;

            typedef typename iterator_type::Pointer pointer;
            typedef const typename iterator_type::Pointer const_pointer;

            typedef iterator_type iterator;
            typedef const iterator_type const_iterator;

            iterator begin() { return first; }
            const_iterator begin() const { return first; }
            iterator end() { return last; }
            const_iterator end() const { return last; }

            reference front() { return *first; }
            const_reference front() const { return *first; }
            reference back() { iterator_type tmp = last; return *(--tmp); }
            const_reference back() const { iterator_type tmp = last; return *(--tmp); }

            bool empty() const { return first == last; }

        private:
            iterator_type first;
            iterator_type last;
        };

    }

}

#endif

<commit_msg>added hook support for container_range_wrapper<commit_after>#ifndef VIENNAGRID_STORAGE_RANGE_HPP
#define VIENNAGRID_STORAGE_RANGE_HPP

namespace viennagrid
{

    namespace storage
    {

        template<typename container_type>
        class container_range_wrapper
        {
            friend class container_range_wrapper<const container_type>;

        public:

            container_range_wrapper(container_type & _container) : container(_container) {}

            typedef typename container_type::size_type size_type;
            typedef typename container_type::value_type value_type;

            typedef typename container_type::reference reference;
            typedef typename container_type::const_reference const_reference;

            typedef typename container_type::pointer pointer;
            typedef typename container_type::const_pointer const_pointer;

            typedef typename container_type::iterator iterator;
            typedef typename container_type::const_iterator const_iterator;

            typedef typename container_type::reverse_iterator reverse_iterator;
            typedef typename container_type::const_reverse_iterator const_reverse_iterator;

            iterator begin() { return container.begin(); }
            const_iterator begin() const { return container.begin(); }
            iterator end() { return container.end(); }
            const_iterator end() const { return container.end(); }

            reverse_iterator rbegin() { return container.rbegin(); }
            const_reverse_iterator rbegin() const { return container.rbegin(); }
            reverse_iterator rend() { return container.rend(); }
            const_reverse_iterator rend() const { return container.rend(); }

            reference front() { return container.front(); }
            const_reference front() const { return container.front(); }
            reference back() { return container.back(); }
            const_reference back() const { return container.back(); }

            reference operator[] (size_type index) { return container[index]; }
            const_reference operator[] (size_type index) const { return container[index]; }

            bool empty() const { return container.empty(); }
            size_type size() const { return container.size(); }

            typedef typename container_type::hook_type hook_type;
            typedef typename container_type::hook_iterator hook_iterator;
            hook_iterator hook_begin() { return container.hook_begin(); }
            hook_iterator hook_end() { return container.hook_end(); }

            typedef typename container_type::const_hook_type const_hook_type;
            typedef typename container_type::const_hook_iterator const_hook_iterator;
            const_hook_iterator hook_begin() const { return container.hook_begin(); }
            const_hook_iterator hook_end() const { return container.hook_end(); }

        private:

            container_type & container;
        };

        template<typename container_type>
        class container_range_wrapper<const container_type>
        {
        public:

            container_range_wrapper(const container_type & _container) : container(_container) {}
            container_range_wrapper(const container_range_wrapper<container_type> & rhs) : container(rhs.container) {}

            typedef typename container_type::size_type size_type;
            typedef typename container_type::value_type value_type;

            typedef typename container_type::const_reference reference;
            typedef typename container_type::const_reference const_reference;

            typedef typename container_type::const_pointer pointer;
            typedef typename container_type::const_pointer const_pointer;

            typedef typename container_type::const_iterator iterator;
            typedef typename container_type::const_iterator const_iterator;

            typedef typename container_type::const_reverse_iterator reverse_iterator;
            typedef typename container_type::const_reverse_iterator const_reverse_iterator;

            iterator begin() { return container.begin(); }
            const_iterator begin() const { return container.begin(); }
            iterator end() { return container.end(); }
            const_iterator end() const { return container.end(); }

            reverse_iterator rbegin() { return container.rbegin(); }
            const_reverse_iterator rbegin() const { return container.rbegin(); }
            reverse_iterator rend() { return container.rend(); }
            const_reverse_iterator rend() const { return container.rend(); }

            reference front() { return container.front(); }
            const_reference front() const { return container.front(); }
            reference back() { return container.back(); }
            const_reference back() const { return container.back(); }

            reference operator[] (size_type index) { return container[index]; }
            const_reference operator[] (size_type index) const { return container[index]; }

            bool empty() const { return container.empty(); }
            size_type size() const { return container.size(); }

            typedef typename container_type::const_hook_type hook_type;
            typedef typename container_type::const_hook_iterator hook_iterator;
            hook_iterator hook_begin() { return container.hook_begin(); }
            hook_iterator hook_end() { return container.hook_end(); }

            typedef typename container_type::const_hook_type const_hook_type;
            typedef typename container_type::const_hook_iterator const_hook_iterator;
            const_hook_iterator hook_begin() const { return container.hook_begin(); }
            const_hook_iterator hook_end() const { return container.hook_end(); }

        private:

            const container_type & container;
        };

        template<typename iterator_type>
        class forward_iterator_range
        {
        public:

            forward_iterator_range(iterator_type _first, iterator_type _last) : first(_first), last(_last) {}

            typedef typename iterator_type::T value_type;

            typedef typename iterator_type::Reference reference;
            typedef const typename iterator_type::Reference const_reference;

            typedef typename iterator_type::Pointer pointer;
            typedef const typename iterator_type::Pointer const_pointer;

            typedef iterator_type iterator;
            typedef const iterator_type const_iterator;

            iterator begin() { return first; }
            const_iterator begin() const { return first; }
            iterator end() { return last; }
            const_iterator end() const { return last; }

            reference front() { return *first; }
            const_reference front() const { return *first; }
            reference back() { iterator_type tmp = last; return *(--tmp); }
            const_reference back() const { iterator_type tmp = last; return *(--tmp); }

            bool empty() const { return first == last; }

        private:
            iterator_type first;
            iterator_type last;
        };

    }

}

#endif

<|endoftext|>


<commit_before>/**
 * @file gmm_test.cpp
 * @author Ryan Curtin
 *
 * Test for the Gaussian Mixture Model class.
 */
#include <mlpack/core.h>

#include <mlpack/methods/gmm/gmm.hpp>
#include <mlpack/methods/gmm/phi.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack;
using namespace mlpack::gmm;

BOOST_AUTO_TEST_SUITE(GMMTest);

/**
 * Test the phi() function, in the univariate Gaussian case.
 */
BOOST_AUTO_TEST_CASE(UnivariatePhiTest)
{
  // Simple case.
  BOOST_REQUIRE_CLOSE(phi(0.0, 0.0, 1.0), 0.398942280401433, 1e-5);

  // A few more cases...
  BOOST_REQUIRE_CLOSE(phi(0.0, 0.0, 2.0), 0.282094791773878, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 0.0, 1.0), 0.241970724519143, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-1.0, 0.0, 1.0), 0.241970724519143, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 0.0, 2.0), 0.219695644733861, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-1.0, 0.0, 2.0), 0.219695644733861, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 1.0, 1.0), 0.398942280401433, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(-1.0, 1.0, 2.0), 0.103776874355149, 1e-5);
}

/**
 * Test the phi() function, in the multivariate Gaussian case.
 */
BOOST_AUTO_TEST_CASE(MultivariatePhiTest)
{
  // Simple case.
  arma::vec mean = "0 0";
  arma::mat cov = "1 0; 0 1";
  arma::vec x = "0 0";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.159154943091895, 1e-5);

  cov = "2 0; 0 2";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0795774715459477, 1e-5);

  x = "1 1";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0482661763150270, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 0.0482661763150270, 1e-5);

  mean = "1 1";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0795774715459477, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 0.0795774715459477, 1e-5);

  cov = "2 1.5; 1 4";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0624257046546403, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 0.0624257046546403, 1e-5);

  x = "-1 4";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.00144014867515135, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 0.00133352162064845, 1e-5);

  // Higher-dimensional case.
  x = "0 1 2 3 4";
  mean = "5 6 3 3 2";
  cov = "6 1 1 0 2;"
        "0 7 1 0 1;"
        "1 1 4 1 1;"
        "1 0 1 7 0;"
        "2 0 1 1 6";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(x, -mean, cov), 1.06784794079363e-8, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 1.06784794079363e-8, 1e-5);
}

/**
 * Test the phi() function, for multiple points in the multivariate Gaussian
 * case.
 */
BOOST_AUTO_TEST_CASE(MultipointMultivariatePhiTest)
{
  // Same case as before.
  arma::vec mean = "5 6 3 3 2";
  arma::mat cov = "6 1 1 0 2; 0 7 1 0 1; 1 1 4 1 1; 1 0 1 7 0; 2 0 1 1 6";

  arma::mat points = "0 3 2 2 3 4;"
                     "1 2 2 1 0 0;"
                     "2 3 0 5 5 6;"
                     "3 7 8 0 1 1;"
                     "4 8 1 1 0 0;";

  arma::vec phis;
  phi(points, mean, cov, phis);

  BOOST_REQUIRE_EQUAL(phis.n_elem, 6);

  BOOST_REQUIRE_CLOSE(phis(0), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(1), 1.82353695848039e-7, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(2), 1.29759261892949e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(3), 1.33218060268258e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(4), 1.12120427975708e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(5), 4.57951032485297e-7, 1e-5);
}

/**
 * Test training a model on only one Gaussian (randomly generated) in two
 * dimensions.  We will vary the dataset size from small to large.  The EM
 * algorithm is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMOneGaussian)
{
  // Initialize random seed.
  srand(time(NULL));

  for (size_t iterations = 0; iterations < 4; iterations++)
  {
    // Determine random covariance and mean.
    arma::vec mean;
    mean.randu(2);
    arma::vec covar;
    covar.randu(2);

    arma::mat data;
    data.randn(2 /* dimension */, 100 * pow(10, (iterations / 3.0)));

    // Now apply mean and covariance.
    data.row(0) *= covar(0);
    data.row(1) *= covar(1);

    data.row(0) += mean(0);
    data.row(1) += mean(1);

    // Now, train the model.
    GMM gmm(1, 2);
    gmm.ExpectationMaximization(data);

    arma::vec actual_mean = arma::mean(data, 1);
    arma::mat actual_covar = ccov(data, 1 /* biased estimator */);

    // Check the model to see that it is correct.
    BOOST_REQUIRE_CLOSE((gmm.Means()[0])[0], actual_mean(0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Means()[0])[1], actual_mean(1), 1e-5);

    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(0, 0), actual_covar(0, 0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(0, 1), actual_covar(0, 1), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(1, 0), actual_covar(1, 0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(1, 1), actual_covar(1, 1), 1e-5);

    BOOST_REQUIRE_CLOSE(gmm.Weights()[0], 1.0, 1e-5);
  }
}

/**
 * Test a training model on multiple Gaussians in higher dimensionality than
 * two.  We will hold the dataset size constant at 10k points.  The EM algorithm
 * is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMMultipleGaussians)
{
  // Higher dimensionality gives us a greater chance of having separated
  // Gaussians.
  size_t dims = 8;
  size_t gaussians = 3;

  // Generate dataset.
  arma::mat data;
  data.zeros(dims, 500);

  std::vector<arma::vec> means(gaussians);
  std::vector<arma::mat> covars(gaussians);
  arma::vec weights(gaussians);
  arma::Col<size_t> counts(gaussians);

  // Choose weights randomly.
  weights.zeros();
  while (weights.min() < 0.02)
  {
    weights.randu(gaussians);
    weights /= accu(weights);
  }

  for (size_t i = 0; i < gaussians; i++)
    counts[i] = round(weights[i] * (data.n_cols - gaussians));
  // Ensure one point minimum in each.
  counts += 1;

  // Account for rounding errors (possibly necessary).
  counts[gaussians - 1] += (data.n_cols - arma::accu(counts));

  // Build each Gaussian individually.
  size_t point = 0;
  for (int i = 0; i < gaussians; i++)
  {
    arma::mat gaussian;
    gaussian.randn(dims, counts[i]);

    // Randomly generate mean and covariance.
    means[i].randu(dims);
    means[i] -= 0.5;
    means[i] *= 50;

    covars[i].randu(dims, dims);
    covars[i] *= 2;

    data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i]
        * arma::ones<arma::rowvec>(counts[i]));

    // Calculate the actual means and covariances because they will probably
    // be different (this is easier to do before we shuffle the points).
    means[i] = arma::mean(data.cols(point, point + counts[i] - 1), 1);
    covars[i] = ccov(data.cols(point, point + counts[i] - 1), 1 /* biased */);

    point += counts[i];
  }

  // Calculate actual weights.
  for (size_t i = 0; i < gaussians; i++)
    weights[i] = (double) counts[i] / data.n_cols;

  // Now train the model.
  GMM gmm(gaussians, dims);
  gmm.ExpectationMaximization(data);

  arma::uvec sort_ref = sort_index(weights);
  arma::uvec sort_try = sort_index(gmm.Weights());

  // Check the model to see that it is correct.
  for (int i = 0; i < gaussians; i++)
  {
    // Check the mean.
    for (int j = 0; j < dims; j++)
      BOOST_REQUIRE_CLOSE((gmm.Means()[sort_try[i]])[j],
          (means[sort_ref[i]])[j], 1e-5);

    // Check the covariance.
    for (int row = 0; row < dims; row++)
      for (int col = 0; col < dims; col++)
        BOOST_REQUIRE_CLOSE((gmm.Covariances()[sort_try[i]])(row, col),
            (covars[sort_ref[i]])(row, col), 1e-5);

    // Check the weight.
    BOOST_REQUIRE_CLOSE(gmm.Weights()[sort_try[i]], weights[sort_ref[i]],
        1e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();
<commit_msg>Fix some warnings I inadvertently introduced.<commit_after>/**
 * @file gmm_test.cpp
 * @author Ryan Curtin
 *
 * Test for the Gaussian Mixture Model class.
 */
#include <mlpack/core.h>

#include <mlpack/methods/gmm/gmm.hpp>
#include <mlpack/methods/gmm/phi.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack;
using namespace mlpack::gmm;

BOOST_AUTO_TEST_SUITE(GMMTest);

/**
 * Test the phi() function, in the univariate Gaussian case.
 */
BOOST_AUTO_TEST_CASE(UnivariatePhiTest)
{
  // Simple case.
  BOOST_REQUIRE_CLOSE(phi(0.0, 0.0, 1.0), 0.398942280401433, 1e-5);

  // A few more cases...
  BOOST_REQUIRE_CLOSE(phi(0.0, 0.0, 2.0), 0.282094791773878, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 0.0, 1.0), 0.241970724519143, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-1.0, 0.0, 1.0), 0.241970724519143, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 0.0, 2.0), 0.219695644733861, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-1.0, 0.0, 2.0), 0.219695644733861, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(1.0, 1.0, 1.0), 0.398942280401433, 1e-5);

  BOOST_REQUIRE_CLOSE(phi(-1.0, 1.0, 2.0), 0.103776874355149, 1e-5);
}

/**
 * Test the phi() function, in the multivariate Gaussian case.
 */
BOOST_AUTO_TEST_CASE(MultivariatePhiTest)
{
  // Simple case.
  arma::vec mean = "0 0";
  arma::mat cov = "1 0; 0 1";
  arma::vec x = "0 0";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.159154943091895, 1e-5);

  cov = "2 0; 0 2";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0795774715459477, 1e-5);

  x = "1 1";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0482661763150270, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 0.0482661763150270, 1e-5);

  mean = "1 1";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0795774715459477, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 0.0795774715459477, 1e-5);

  cov = "2 1.5; 1 4";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.0624257046546403, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 0.0624257046546403, 1e-5);

  x = "-1 4";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 0.00144014867515135, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 0.00133352162064845, 1e-5);

  // Higher-dimensional case.
  x = "0 1 2 3 4";
  mean = "5 6 3 3 2";
  cov = "6 1 1 0 2;"
        "0 7 1 0 1;"
        "1 1 4 1 1;"
        "1 0 1 7 0;"
        "2 0 1 1 6";

  BOOST_REQUIRE_CLOSE(phi(x, mean, cov), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, -mean, cov), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(x, -mean, cov), 1.06784794079363e-8, 1e-5);
  BOOST_REQUIRE_CLOSE(phi(-x, mean, cov), 1.06784794079363e-8, 1e-5);
}

/**
 * Test the phi() function, for multiple points in the multivariate Gaussian
 * case.
 */
BOOST_AUTO_TEST_CASE(MultipointMultivariatePhiTest)
{
  // Same case as before.
  arma::vec mean = "5 6 3 3 2";
  arma::mat cov = "6 1 1 0 2; 0 7 1 0 1; 1 1 4 1 1; 1 0 1 7 0; 2 0 1 1 6";

  arma::mat points = "0 3 2 2 3 4;"
                     "1 2 2 1 0 0;"
                     "2 3 0 5 5 6;"
                     "3 7 8 0 1 1;"
                     "4 8 1 1 0 0;";

  arma::vec phis;
  phi(points, mean, cov, phis);

  BOOST_REQUIRE_EQUAL(phis.n_elem, 6);

  BOOST_REQUIRE_CLOSE(phis(0), 1.02531207499358e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(1), 1.82353695848039e-7, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(2), 1.29759261892949e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(3), 1.33218060268258e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(4), 1.12120427975708e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(5), 4.57951032485297e-7, 1e-5);
}

/**
 * Test training a model on only one Gaussian (randomly generated) in two
 * dimensions.  We will vary the dataset size from small to large.  The EM
 * algorithm is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMOneGaussian)
{
  // Initialize random seed.
  srand(time(NULL));

  for (size_t iterations = 0; iterations < 4; iterations++)
  {
    // Determine random covariance and mean.
    arma::vec mean;
    mean.randu(2);
    arma::vec covar;
    covar.randu(2);

    arma::mat data;
    data.randn(2 /* dimension */, 100 * pow(10, (iterations / 3.0)));

    // Now apply mean and covariance.
    data.row(0) *= covar(0);
    data.row(1) *= covar(1);

    data.row(0) += mean(0);
    data.row(1) += mean(1);

    // Now, train the model.
    GMM gmm(1, 2);
    gmm.ExpectationMaximization(data);

    arma::vec actual_mean = arma::mean(data, 1);
    arma::mat actual_covar = ccov(data, 1 /* biased estimator */);

    // Check the model to see that it is correct.
    BOOST_REQUIRE_CLOSE((gmm.Means()[0])[0], actual_mean(0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Means()[0])[1], actual_mean(1), 1e-5);

    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(0, 0), actual_covar(0, 0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(0, 1), actual_covar(0, 1), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(1, 0), actual_covar(1, 0), 1e-5);
    BOOST_REQUIRE_CLOSE((gmm.Covariances()[0])(1, 1), actual_covar(1, 1), 1e-5);

    BOOST_REQUIRE_CLOSE(gmm.Weights()[0], 1.0, 1e-5);
  }
}

/**
 * Test a training model on multiple Gaussians in higher dimensionality than
 * two.  We will hold the dataset size constant at 10k points.  The EM algorithm
 * is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMMultipleGaussians)
{
  // Higher dimensionality gives us a greater chance of having separated
  // Gaussians.
  size_t dims = 8;
  size_t gaussians = 3;

  // Generate dataset.
  arma::mat data;
  data.zeros(dims, 500);

  std::vector<arma::vec> means(gaussians);
  std::vector<arma::mat> covars(gaussians);
  arma::vec weights(gaussians);
  arma::Col<size_t> counts(gaussians);

  // Choose weights randomly.
  weights.zeros();
  while (weights.min() < 0.02)
  {
    weights.randu(gaussians);
    weights /= accu(weights);
  }

  for (size_t i = 0; i < gaussians; i++)
    counts[i] = round(weights[i] * (data.n_cols - gaussians));
  // Ensure one point minimum in each.
  counts += 1;

  // Account for rounding errors (possibly necessary).
  counts[gaussians - 1] += (data.n_cols - arma::accu(counts));

  // Build each Gaussian individually.
  size_t point = 0;
  for (size_t i = 0; i < gaussians; i++)
  {
    arma::mat gaussian;
    gaussian.randn(dims, counts[i]);

    // Randomly generate mean and covariance.
    means[i].randu(dims);
    means[i] -= 0.5;
    means[i] *= 50;

    covars[i].randu(dims, dims);
    covars[i] *= 2;

    data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i]
        * arma::ones<arma::rowvec>(counts[i]));

    // Calculate the actual means and covariances because they will probably
    // be different (this is easier to do before we shuffle the points).
    means[i] = arma::mean(data.cols(point, point + counts[i] - 1), 1);
    covars[i] = ccov(data.cols(point, point + counts[i] - 1), 1 /* biased */);

    point += counts[i];
  }

  // Calculate actual weights.
  for (size_t i = 0; i < gaussians; i++)
    weights[i] = (double) counts[i] / data.n_cols;

  // Now train the model.
  GMM gmm(gaussians, dims);
  gmm.ExpectationMaximization(data);

  arma::uvec sort_ref = sort_index(weights);
  arma::uvec sort_try = sort_index(gmm.Weights());

  // Check the model to see that it is correct.
  for (size_t i = 0; i < gaussians; i++)
  {
    // Check the mean.
    for (size_t j = 0; j < dims; j++)
      BOOST_REQUIRE_CLOSE((gmm.Means()[sort_try[i]])[j],
          (means[sort_ref[i]])[j], 1e-5);

    // Check the covariance.
    for (size_t row = 0; row < dims; row++)
      for (size_t col = 0; col < dims; col++)
        BOOST_REQUIRE_CLOSE((gmm.Covariances()[sort_try[i]])(row, col),
            (covars[sort_ref[i]])(row, col), 1e-5);

    // Check the weight.
    BOOST_REQUIRE_CLOSE(gmm.Weights()[sort_try[i]], weights[sort_ref[i]],
        1e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>

#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>

#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>

#include "navigator.h"
#include "rtl.h"

RTL::RTL(Navigator *navigator, const char *name) :
	NavigatorMode(navigator, name),
	MissionBlock(navigator),
	_rtl_state(RTL_STATE_NONE),
	_param_return_alt(this, "RETURN_ALT"),
	_param_descend_alt(this, "DESCEND_ALT"),
	_param_land_delay(this, "LAND_DELAY"),
	_param_acceptance_radius(this, "ACCEPT_RAD")
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	_first_run = true;
	_rtl_state = RTL_STATE_NONE;
}

bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
	bool updated = false;

	if (_first_run) {
		set_rtl_item(pos_sp_triplet);
		updated = true;
		_first_run = false;
	}

	if ((_rtl_state == RTL_STATE_CLIMB
	     || _rtl_state == RTL_STATE_RETURN)
	    && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item(pos_sp_triplet);
		updated = true;
	}

	return updated;
}

void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
{
	/* make sure we have the latest params */
	updateParams();

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_vstatus()->condition_landed) {
			_rtl_state = RTL_STATE_FINISHED;
			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
		/* if lower than return altitude, climb up first */
		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
			   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;
		/* otherwise go straight to return */
		} else {
			_rtl_state = RTL_STATE_RETURN;
		}
	}

	/* if switching directly to return state, set altitude setpoint to current altitude */
	if (_rtl_state == RTL_STATE_RETURN) {
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_global_position()->alt;
	}

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = climb_alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
			(int)(climb_alt - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_RETURN: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;

		/* TODO: add this again */
		// don't change altitude
		// if (_pos_sp_triplet.previous.valid) {
		// 	/* if previous setpoint is valid then use it to calculate heading to home */
		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);

		// } else {
		// 	/* else use current position */
		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
		// }
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_DESCEND: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(true);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}

	case RTL_STATE_LAND: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LAND;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
		break;
	}

	case RTL_STATE_FINISHED: {
		/* nothing to do, report fail */
	}

	default:
		break;
	}

	if (_rtl_state == RTL_STATE_FINISHED) {
		pos_sp_triplet->current.valid = false;
		pos_sp_triplet->next.valid = false;
	} else {
		/* if not finished, convert mission item to current position setpoint and make it valid */
		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
		reset_mission_item_reached();
		pos_sp_triplet->current.valid = true;
		pos_sp_triplet->next.valid = false;
	}
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;
		break;

	case RTL_STATE_DESCEND:
		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < 0) {
			_rtl_state = RTL_STATE_FINISHED;
		} else {
			_rtl_state = RTL_STATE_LAND;
		}
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_FINISHED;
		break;

	case RTL_STATE_FINISHED:
		break;

	default:
		break;
	}
}
<commit_msg>navigator: RTL return altitude fixed<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>

#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>

#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>

#include "navigator.h"
#include "rtl.h"

RTL::RTL(Navigator *navigator, const char *name) :
	NavigatorMode(navigator, name),
	MissionBlock(navigator),
	_rtl_state(RTL_STATE_NONE),
	_param_return_alt(this, "RETURN_ALT"),
	_param_descend_alt(this, "DESCEND_ALT"),
	_param_land_delay(this, "LAND_DELAY"),
	_param_acceptance_radius(this, "ACCEPT_RAD")
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	_first_run = true;
	_rtl_state = RTL_STATE_NONE;
}

bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
	bool updated = false;

	if (_first_run) {
		set_rtl_item(pos_sp_triplet);
		updated = true;
		_first_run = false;
	}

	if ((_rtl_state == RTL_STATE_CLIMB
	     || _rtl_state == RTL_STATE_RETURN)
	    && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item(pos_sp_triplet);
		updated = true;
	}

	return updated;
}

void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
{
	/* make sure we have the latest params */
	updateParams();

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_vstatus()->condition_landed) {
			_rtl_state = RTL_STATE_FINISHED;
			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");

		/* if lower than return altitude, climb up first */
		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
			   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */
		} else {
		    /* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
	        _mission_item.altitude_is_relative = false;
	        _mission_item.altitude = _navigator->get_global_position()->alt;
		}
	}

    if (_rtl_state == RTL_STATE_FINISHED) {
        /* RTL finished, nothing to do */
        pos_sp_triplet->current.valid = false;
        pos_sp_triplet->next.valid = false;
        return;
    }

    set_previous_pos_setpoint(pos_sp_triplet);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = climb_alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
			(int)(climb_alt - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_RETURN: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		 // don't change altitude

		 if (pos_sp_triplet->previous.valid) {
		 	/* if previous setpoint is valid then use it to calculate heading to home */
		 	_mission_item.yaw = get_bearing_to_next_waypoint(
		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
		 	        _mission_item.lat, _mission_item.lon);

		 } else {
		 	/* else use current position */
		 	_mission_item.yaw = get_bearing_to_next_waypoint(
		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
		 	        _mission_item.lat, _mission_item.lon);
		 }
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_DESCEND: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(true);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}

	case RTL_STATE_LAND: {

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LAND;
		_mission_item.acceptance_radius = _param_acceptance_radius.get();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_is_in_loiter(false);

		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
		break;
	}

	default:
		break;
	}

    /* convert mission item to current position setpoint and make it valid */
    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
    reset_mission_item_reached();
    pos_sp_triplet->current.valid = true;
    pos_sp_triplet->next.valid = false;
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;
		break;

	case RTL_STATE_DESCEND:
		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < 0) {
			_rtl_state = RTL_STATE_FINISHED;
		} else {
			_rtl_state = RTL_STATE_LAND;
		}
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_FINISHED;
		break;

	case RTL_STATE_FINISHED:
		break;

	default:
		break;
	}
}
<|endoftext|>