<commit_before>#include "gtest/gtest.h"
#include "Activation.h"
#include "HiddenLayer.h"
#include "Sigmoid.h"
#include "TanH.h"

#include <vector>
#include "util.hpp"
#include "OpenCL.hpp"
#include "CLMatrix.hpp"
#include "FeedForwardNN.h"

using namespace std;

TEST(Matrix,init){
    int r = 25;
    int c = 30;
    CL_Matrix<float> mat(r,c);
    mat.zeros();
    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),0);
        }
    }
    // Randomize matrix
    mat.random(0.5,1);

    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_NEAR(mat(i,j),0.75,0.25);
        }
    }

}

// TEST(Matrix,shuffle){
//     Matrix m(4,4,true);
//     // std::cout << m;
//     // std::cout << std::endl;
//     // m.shuffle(true);

//     std::cout << m ;

//     m.shuffle(false);
//     std::cout << std::endl;

//     std::cout << m;
// }

TEST(Matrix,transpose){
    int r = 25;
    int c = 30;
    CL_Matrix<float> mat(r,c);
    mat.random(0,1);
    CL_Matrix<float> transpose = mat.transpose();

    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(transpose(j,i),mat(i,j));
        }
    }
}

TEST(Matrix,nonaligned){
    int r1=5;
    int c1=1;

    int r2 = 2;
    int c2 = 3;
    CL_Matrix<float> mat1(r1,c1);
    CL_Matrix<float> mat2(r2,c2);

    EXPECT_DEATH(mat1 += mat2,".*Assertion.*failed.*");

    EXPECT_DEATH(mat1 -= mat2,".*Assertion.*failed.*");

    EXPECT_DEATH(mat1.dot(mat2),".*Assertion.*failed.*");

}

TEST(Matrix,subvector){
	auto r = 10u;
	auto c = 6u;
	CL_Matrix<float> mat (r,c);
	mat.fill(2.);
	CL_Matrix<float> subcol = mat.subMatCol(2);

	EXPECT_EQ(subcol.getCols(),1);
	EXPECT_EQ(subcol.getRows(),r);

	CL_Matrix<float> subrow = mat.subMatRow(2);
//
	EXPECT_EQ(subrow.getCols(),c);
	EXPECT_EQ(subrow.getRows(),1);
//
	for(auto i=0u; i < r ; i++){
		EXPECT_EQ(subcol(i,0),2.);
	}
	for(auto i=0u; i < c ; i++){
		EXPECT_EQ(subrow(0,i),2.);
	}

}

TEST(Matrix,addsubmul){
    auto r=20u;
    auto c=20u;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(r,c);

    mat.fill(5.);

    other.fill(3.);

    mat -= other;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),2.);
        }
    }

    mat+=mat += mat;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),8.);
        }
    }

    mat *= mat;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),64.);
        }
    }

    CL_Matrix<float> addres = mat + mat;
    for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_EQ(addres(i,j),128.);
		}
	}

    CL_Matrix<float> subres = mat - mat;
    for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_EQ(subres(i,j),0.);
		}
	}
}

TEST(Matrix,mult){
	auto r=128u;
	auto c=128u;

	CL_Matrix<float> mat(r,c);
	mat.random(0,1);
	CL_Matrix<float> other(r,c);
	other.random(0,1);

	CL_Matrix<float> res = mat*other;
	for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_NEAR(res(i,j),0.5,0.5);
		}
	}

}

TEST(OpenCL,Datatypes){
//	Testing all the different datatypes
    OpenCL a("kernels.cl") ;

    const int size = 15;
    std::vector<std::size_t> globsize = {size};
    std::vector<std::size_t> localsize = {1};
    float b[size] = {0};
    float c[size] = {0};

    std::vector<std::size_t> outputargs = {2};

    a.runKernel("sigmoid",outputargs,globsize,localsize,size,b,c);

    std::vector<float> d(size);
    std::vector<float> e(size);
    a.runKernel("sigmoid",outputargs,globsize,localsize,size,d,e);

    for (int i = 0; i < size; ++i)
    {
        EXPECT_EQ(c[i],0.5);
        EXPECT_EQ(e[i],0.5);
    }

}

TEST(Matrix,dotsq){

    int r=512;
    int c=512;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotuneven){

    int r=256;
    int c=512;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotvector){

    auto r=1u;
    auto c=1024u;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotvectordiff){

    auto r1=10u;
    auto c1=1u;

    auto r2=1u;
    auto c2=2u;

    CL_Matrix<float> mat(r1,c1);
    CL_Matrix<float> other(r2,c2);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);
    long count = 0;
    for(unsigned i = 0; i < r1; ++i) {
        for(unsigned j = 0; j < c2; ++j) {
            if (out(i,j)!= 10.){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Activation,Sigmoid){
    auto length = 10u;
    CL_Matrix<float> mat(length,1);
    mat.fill(0.);
	Sigmoid s;
	CL_Matrix<float> out = s.propagate(mat);
    for (auto i = 0u; i < length; ++i)
    {
        EXPECT_EQ(out(i,0),0.5);
    }

    CL_Matrix<float> mat2(length,length);

    mat2.fill(0.);
    CL_Matrix<float> out2 = s.propagate(mat2);
    for(auto i=0u; i < length ; i++){
    	for(auto j=0u; j < length ; j++){
    		EXPECT_EQ(out2(i,j),0.5);
		}
    }

}

TEST(Activation,Sigmoidgrad){
	auto size= 20u;
	CL_Matrix<float> f(size,size);

	CL_Matrix<float> res = f.sigmoidgrad();

	for(auto i=0u; i < size ; i++){
		for(auto j=0u; j < size ; j++){
			EXPECT_EQ(res(i,j),0.25);
		}
	}
}

TEST(Activation,Tanh){
    int length = 100;
    CL_Matrix<float> mat(length,1);
    mat.fill(1.);
	TanH tan;
	CL_Matrix<float> out = tan.propagate(mat);
    for (int i = 0; i < length; ++i)
    {
        EXPECT_NEAR(out(i,0),0.76,0.02);
    }
}

TEST(Nnet,batchgradient){
	CL_Matrix<float> input(3,3);
	input.fill(0.1);
	CL_Matrix<float> target(2,3);
	target.fill(1.);
	Sigmoid s;
	FeedForwardNN dnn(3,2,0.1);
	dnn.addActivation(&s);
	dnn.addActivation(&s);
	dnn.addHiddenLayer(5);
	std::vector<float> errors = dnn.trainbatch(input,target);
    for (auto i = 1u; i < errors.size(); ++i)
    {
        EXPECT_GE(errors[i-1] - errors[i]  ,0);
    }
}

TEST(Nnet,sgd){
	CL_Matrix<float> input(3,3);
	input.fill(0.);
	CL_Matrix<float> target(2,1);
	target.fill(1.);
	Sigmoid s;
	FeedForwardNN dnn(3,2,0.5,0.1);
	dnn.addActivation(&s);
	dnn.addActivation(&s);
	dnn.addHiddenLayer(5);
	std::vector<float> errors = dnn.trainsgd(input,target);
    for (auto i = 1u; i < errors.size(); ++i)
    {
        EXPECT_GE(errors[i-1] - errors[i]  ,0);
    }
}

int main(int argc,char **argv){
    ::testing::InitGoogleTest(&argc, argv);
//	::testing::GTEST_FLAG(filter) = "Matrix.dotvectordiff:Matrix.dotvector";//":-:*Counter*";
    // Otherwise EXPECT_DEATH will warn us that the execution time may be too slow,
    // since EXPECT_DEATH uses forks, which could not be used
    ::testing::FLAGS_gtest_death_test_style = "threadsafe";
    return RUN_ALL_TESTS();

}

<commit_msg>Added new tests, dotgpu, where the application of the on gpu matrix mult can be seen<commit_after>#include "gtest/gtest.h"
#include "Activation.h"
#include "HiddenLayer.h"
#include "Sigmoid.h"
#include "TanH.h"

#include <vector>
#include "util.hpp"
#include "OpenCL.hpp"
#include "CLMatrix.hpp"
#include "FeedForwardNN.h"

using namespace std;

TEST(Matrix,init){
    int r = 25;
    int c = 30;
    CL_Matrix<float> mat(r,c);
    mat.zeros();
    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),0);
        }
    }
    // Randomize matrix
    mat.random(0.5,1);

    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_NEAR(mat(i,j),0.75,0.25);
        }
    }

}

// TEST(Matrix,shuffle){
//     Matrix m(4,4,true);
//     // std::cout << m;
//     // std::cout << std::endl;
//     // m.shuffle(true);

//     std::cout << m ;

//     m.shuffle(false);
//     std::cout << std::endl;

//     std::cout << m;
// }

TEST(Matrix,transpose){
    int r = 25;
    int c = 30;
    CL_Matrix<float> mat(r,c);
    mat.random(0,1);
    CL_Matrix<float> transpose = mat.transpose();

    for (int i = 0; i < r; ++i)
    {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(transpose(j,i),mat(i,j));
        }
    }
}

TEST(Matrix,nonaligned){
    int r1=5;
    int c1=1;

    int r2 = 2;
    int c2 = 3;
    CL_Matrix<float> mat1(r1,c1);
    CL_Matrix<float> mat2(r2,c2);

    EXPECT_DEATH(mat1 += mat2,".*Assertion.*failed.*");
//
    EXPECT_DEATH(mat1 -= mat2,".*Assertion.*failed.*");
//
//
    EXPECT_DEATH(mat1.dot(mat2),".*Assertion.*failed.*");

}

TEST(Matrix,subvector){
	auto r = 10u;
	auto c = 6u;
	CL_Matrix<float> mat (r,c);
	mat.fill(2.);
	CL_Matrix<float> subcol = mat.subMatCol(2);

	EXPECT_EQ(subcol.getCols(),1);
	EXPECT_EQ(subcol.getRows(),r);

	CL_Matrix<float> subrow = mat.subMatRow(2);
//
	EXPECT_EQ(subrow.getCols(),c);
	EXPECT_EQ(subrow.getRows(),1);
//
	for(auto i=0u; i < r ; i++){
		EXPECT_EQ(subcol(i,0),2.);
	}
	for(auto i=0u; i < c ; i++){
		EXPECT_EQ(subrow(0,i),2.);
	}

}

TEST(Matrix,addsubmul){
    auto r=20u;
    auto c=20u;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(r,c);

    mat.fill(5.);

    other.fill(3.);

    mat -= other;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),2.);
        }
    }

    mat+=mat += mat;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),8.);
        }
    }

    mat *= mat;

    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < c; ++j) {
            EXPECT_EQ(mat(i,j),64.);
        }
    }

    CL_Matrix<float> addres = mat + mat;
    for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_EQ(addres(i,j),128.);
		}
	}

    CL_Matrix<float> subres = mat - mat;
    for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_EQ(subres(i,j),0.);
		}
	}
}

TEST(Matrix,mult){
	auto r=128u;
	auto c=128u;

	CL_Matrix<float> mat(r,c);
	mat.random(0,1);
	CL_Matrix<float> other(r,c);
	other.random(0,1);

	CL_Matrix<float> res = mat*other;
	for(unsigned i = 0; i < r; ++i) {
		for(unsigned j = 0; j < c; ++j) {
			EXPECT_NEAR(res(i,j),0.5,0.5);
		}
	}

}

TEST(OpenCL,Datatypes){
//	Testing all the different datatypes
    OpenCL a("kernels.cl") ;

    const int size = 15;
    std::vector<std::size_t> globsize = {size};
    std::vector<std::size_t> localsize = {1};
    float b[size] = {0};
    float c[size] = {0};

    std::vector<std::size_t> outputargs = {2};

    a.runKernel("sigmoid",outputargs,globsize,localsize,size,b,c);

    std::vector<float> d(size);
    std::vector<float> e(size);
    a.runKernel("sigmoid",outputargs,globsize,localsize,size,d,e);

    for (int i = 0; i < size; ++i)
    {
        EXPECT_EQ(c[i],0.5);
        EXPECT_EQ(e[i],0.5);
    }

}

TEST(Matrix,dotgpu){

    int r=20;
    int c=20;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dotgpu(other);
    for(auto i=0u;i<5;i++){
		out = out.dotgpu(other);
	}
	out.fetchdata();

}

TEST(Matrix,dotsq){

    int r=512;
    int c=512;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotuneven){

    int r=256;
    int c=512;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotvector){

    auto r=1u;
    auto c=1024u;

    CL_Matrix<float> mat(r,c);
    CL_Matrix<float> other(c,r);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);

    long count = 0;
    for(unsigned i = 0; i < r; ++i) {
        for(unsigned j = 0; j < r; ++j) {
            if (out(i,j)!= c*10){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Matrix,dotvectordiff){

    auto r1=10u;
    auto c1=1u;

    auto r2=1u;
    auto c2=2u;

    CL_Matrix<float> mat(r1,c1);
    CL_Matrix<float> other(r2,c2);
    mat.fill(5.);

    other.fill(2.);
    CL_Matrix<float> out = mat.dot(other);
    long count = 0;
    for(unsigned i = 0; i < r1; ++i) {
        for(unsigned j = 0; j < c2; ++j) {
            if (out(i,j)!= 10.){
                count ++ ;
            }
        }
    }
    EXPECT_EQ(count,0);

}

TEST(Activation,Sigmoid){
    auto length = 1000u;
    CL_Matrix<float> mat(length,1);
    mat.fill(0.);
	Sigmoid s;
	CL_Matrix<float> out = s.propagate(mat);
    for (auto i = 0u; i < length; ++i)
    {
        EXPECT_EQ(out(i,0),0.5);
    }
}

TEST(Activation,SigmoidCPU){
    auto length = 1000u;
    CL_Matrix<float> mat(length,1);
    mat.fill(0.);
	CL_Matrix<float> out = mat.sigmoidcpu();
    for (auto i = 0u; i < length; ++i)
    {
        EXPECT_EQ(out(i,0),0.5);
    }

}

TEST(Activation,Sigmoidgrad){
	auto size= 20u;
	CL_Matrix<float> f(size,size);

	CL_Matrix<float> res = f.sigmoidgrad();

	for(auto i=0u; i < size ; i++){
		for(auto j=0u; j < size ; j++){
			EXPECT_EQ(res(i,j),0.25);
		}
	}
}

TEST(Activation,Tanh){
    int length = 100;
    CL_Matrix<float> mat(length,1);
    mat.fill(1.);
	TanH tan;
	CL_Matrix<float> out = tan.propagate(mat);
    for (int i = 0; i < length; ++i)
    {
        EXPECT_NEAR(out(i,0),0.76,0.02);
    }
}

TEST(Nnet,batchgradient){
	CL_Matrix<float> input(3,3);
	input.fill(0.1);
	CL_Matrix<float> target(2,3);
	target.fill(1.);
	Sigmoid s;
	FeedForwardNN dnn(3,2,0.1);
	dnn.addActivation(&s);
	dnn.addActivation(&s);
	dnn.addHiddenLayer(5);
	std::vector<float> errors = dnn.trainbatch(input,target);
    for (auto i = 1u; i < errors.size(); ++i)
    {
        EXPECT_GE(errors[i-1] - errors[i]  ,0);
    }
}

TEST(Nnet,sgd){
	CL_Matrix<float> input(3,3);
	input.fill(0.);
	CL_Matrix<float> target(2,1);
	target.fill(1.);
	Sigmoid s;
	FeedForwardNN dnn(3,2,0.5,0.1);
	dnn.addActivation(&s);
	dnn.addActivation(&s);
	dnn.addHiddenLayer(5);
	std::vector<float> errors = dnn.trainsgd(input,target);
    for (auto i = 1u; i < errors.size(); ++i)
    {
        EXPECT_GE(errors[i-1] - errors[i]  ,0);
    }
}

int main(int argc,char **argv){
    ::testing::InitGoogleTest(&argc, argv);
	::testing::GTEST_FLAG(filter) = "Matrix.*";//":-:*Counter*";
    // Otherwise EXPECT_DEATH will warn us that the execution time may be too slow,
    // since EXPECT_DEATH uses forks, which could not be used
    ::testing::FLAGS_gtest_death_test_style = "threadsafe";
    return RUN_ALL_TESTS();

}

<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2013 Heiko Strathmann
 */

#ifdef HAVE_EIGEN3

#include <shogun/distributions/classical/GaussianDistribution.h>
#include <shogun/base/Parameter.h>
#include <shogun/mathematics/eigen3.h>

using namespace shogun;
using namespace Eigen;

CGaussianDistribution::CGaussianDistribution() : CProbabilityDistribution()
{
	init();
}

CGaussianDistribution::CGaussianDistribution(SGVector<float64_t> mean,
		SGMatrix<float64_t> cov, bool cov_is_factor) :
				CProbabilityDistribution(mean.vlen)
{
	REQUIRE(cov.num_rows==cov.num_cols, "Covariance must be square but is "
			"%dx%d\n", cov.num_rows, cov.num_cols);
	REQUIRE(mean.vlen==cov.num_cols, "Mean must have same dimension as "
			"covariance, which is %dx%d, but is %d\n",
			cov.num_rows, cov.num_cols, mean.vlen);

	init();

	m_mean=mean;

	if (!cov_is_factor)
	{
		Map<MatrixXd> eigen_cov(cov.matrix, cov.num_rows, cov.num_cols);
		m_L=SGMatrix<float64_t>(cov.num_rows, cov.num_cols);
		Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);

		/* try to compute Cholesky and increase ridge on diagonal otherwise */
		LLT<MatrixXd> llt;
		while (true)
		{
			llt.compute(eigen_cov);
			if (llt.info()==NumericalIssue)
			{
				/* try to compute smalles eigenvalue for information */
				SelfAdjointEigenSolver<MatrixXd> solver(eigen_cov);
				if (solver.info() == Success)
				{
					VectorXd ev=solver.eigenvalues();
					SG_WARNING("Error computing Cholesky of Gaussian's covariance. "
							"Smallest Eigenvalue is %f. Increasing ridge by "
							"%f\n", ev[0], CMath::abs(ev[0]))+1e-3;

					for (index_t i=0; i<cov.num_rows; ++i)
						cov(i,i)+=CMath::abs(ev[0])+1e-3;
				}
			}
			else
				break;
		}

		eigen_L=llt.matrixL();
	}
	else
		m_L=cov;
}

CGaussianDistribution::~CGaussianDistribution()
{

}

SGMatrix<float64_t> CGaussianDistribution::sample(int32_t num_samples,
		SGMatrix<float64_t> pre_samples) const
{
	REQUIRE(num_samples>0, "Number of samples (%d) must be positive\n",
			num_samples);

	/* use pre-allocated samples? */
	SGMatrix<float64_t> samples;
	if (pre_samples.matrix)
	{
		REQUIRE(pre_samples.num_rows==m_dimension, "Dimension of pre-samples"
				" (%d) does not match dimension of Gaussian (%d)\n",
				pre_samples.num_rows, m_dimension);

		REQUIRE(pre_samples.num_cols==num_samples, "Number of pre-samples"
				" (%d) does not desired number of samples (%d)\n",
				pre_samples.num_cols, num_samples);

		samples=pre_samples;
	}
	else
	{
		/* allocate memory and sample from std normal */
		samples=SGMatrix<float64_t>(m_dimension, num_samples);
		for (index_t i=0; i<m_dimension*num_samples; ++i)
			samples.matrix[i]=sg_rand->std_normal_distrib();
	}

	/* map into desired Gaussian covariance */
	Map<MatrixXd> eigen_samples(samples.matrix, samples.num_rows,
			samples.num_cols);
	Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);
	eigen_samples=eigen_L*eigen_samples;

	/* add mean */
	Map<VectorXd> eigen_mean(m_mean.vector, m_mean.vlen);
	eigen_samples.colwise()+=eigen_mean;

	return samples;
}

SGVector<float64_t> CGaussianDistribution::log_pdf(SGMatrix<float64_t> samples) const
{
	REQUIRE(samples.num_cols>0, "Number of samples must be positive, but is %d\n",
			samples.num_cols);
	REQUIRE(samples.num_rows=m_dimension, "Sample dimension (%d) does not match"
			"Gaussian dimension (%d)\n", samples.num_rows, m_dimension);

	/* for easier to read code */
	index_t num_samples=samples.num_cols;

	float64_t const_part=-0.5 * m_dimension * CMath::log(2 * CMath::PI);

	/* determinant is product of diagonal elements of triangular matrix */
	float64_t log_det_part=0;
	Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);
	VectorXd diag=eigen_L.diagonal();
	log_det_part=-diag.array().log().sum();

	/* sample based part */
	Map<MatrixXd> eigen_samples(samples.matrix, samples.num_rows,
			samples.num_cols);
	Map<VectorXd> eigen_mean(m_mean.vector, m_mean.vlen);

	/* substract mean from samples (create copy) */
	SGMatrix<float64_t> centred(m_dimension, num_samples);
	Map<MatrixXd> eigen_centred(centred.matrix, centred.num_rows,
			centred.num_cols);
	for (index_t dim=0; dim<m_dimension; ++dim)
	{
		for (index_t sample_idx=0; sample_idx<num_samples; ++sample_idx)
			centred(dim,sample_idx)=samples(dim,sample_idx)-m_mean[dim];
	}

	/* solve the linear system based on factorization */
	MatrixXd solved=eigen_L.triangularView<Lower>().solve(eigen_centred);
	solved=eigen_L.transpose().triangularView<Upper>().solve(solved);

	/* one quadratic part x^T C^-1 x for each sample x */
	SGVector<float64_t> result(num_samples);
	Map<VectorXd> eigen_result(result.vector, result.vlen);
	for (index_t i=0; i<num_samples; ++i)
	{
		/* i-th centred sample */
		VectorXd left=eigen_centred.block(0, i, m_dimension, 1);

		/* inverted covariance times i-th centred sample */
		VectorXd right=solved.block(0,i,m_dimension,1);
		result[i]=-0.5*left.dot(right);
	}

	/* combine and return */
	eigen_result=eigen_result.array()+(log_det_part+const_part);

	/* contains everything */
	return result;

}

void CGaussianDistribution::init()
{
	SG_ADD(&m_mean, "mean", "Mean of the Gaussian.", MS_NOT_AVAILABLE);
	SG_ADD(&m_L, "L", "Lower factor of covariance matrix, "
			"depending on the factorization type.", MS_NOT_AVAILABLE);
}

#endif // HAVE_EIGEN3
<commit_msg>removed hacky psd checks, user has to do this. Print smallest eigenvalue instead<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2013 Heiko Strathmann
 */

#ifdef HAVE_EIGEN3

#include <shogun/distributions/classical/GaussianDistribution.h>
#include <shogun/base/Parameter.h>
#include <shogun/mathematics/eigen3.h>

using namespace shogun;
using namespace Eigen;

CGaussianDistribution::CGaussianDistribution() : CProbabilityDistribution()
{
	init();
}

CGaussianDistribution::CGaussianDistribution(SGVector<float64_t> mean,
		SGMatrix<float64_t> cov, bool cov_is_factor) :
				CProbabilityDistribution(mean.vlen)
{
	REQUIRE(cov.num_rows==cov.num_cols, "Covariance must be square but is "
			"%dx%d\n", cov.num_rows, cov.num_cols);
	REQUIRE(mean.vlen==cov.num_cols, "Mean must have same dimension as "
			"covariance, which is %dx%d, but is %d\n",
			cov.num_rows, cov.num_cols, mean.vlen);

	init();

	m_mean=mean;

	if (!cov_is_factor)
	{
		Map<MatrixXd> eigen_cov(cov.matrix, cov.num_rows, cov.num_cols);
		m_L=SGMatrix<float64_t>(cov.num_rows, cov.num_cols);
		Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);

		/* compute cholesky */
		LLT<MatrixXd> llt(eigen_cov);
		if (llt.info()==NumericalIssue)
		{
			/* try to compute smalles eigenvalue for information */
			SelfAdjointEigenSolver<MatrixXd> solver(eigen_cov);
			if (solver.info() == Success)
			{
				VectorXd ev=solver.eigenvalues();
				SG_ERROR("Error computing Cholesky of Gaussian's covariance. "
						"Smallest Eigenvalue is %f.\n", ev[0]);
			}
		}

		eigen_L=llt.matrixL();
	}
	else
		m_L=cov;
}

CGaussianDistribution::~CGaussianDistribution()
{

}

SGMatrix<float64_t> CGaussianDistribution::sample(int32_t num_samples,
		SGMatrix<float64_t> pre_samples) const
{
	REQUIRE(num_samples>0, "Number of samples (%d) must be positive\n",
			num_samples);

	/* use pre-allocated samples? */
	SGMatrix<float64_t> samples;
	if (pre_samples.matrix)
	{
		REQUIRE(pre_samples.num_rows==m_dimension, "Dimension of pre-samples"
				" (%d) does not match dimension of Gaussian (%d)\n",
				pre_samples.num_rows, m_dimension);

		REQUIRE(pre_samples.num_cols==num_samples, "Number of pre-samples"
				" (%d) does not desired number of samples (%d)\n",
				pre_samples.num_cols, num_samples);

		samples=pre_samples;
	}
	else
	{
		/* allocate memory and sample from std normal */
		samples=SGMatrix<float64_t>(m_dimension, num_samples);
		for (index_t i=0; i<m_dimension*num_samples; ++i)
			samples.matrix[i]=sg_rand->std_normal_distrib();
	}

	/* map into desired Gaussian covariance */
	Map<MatrixXd> eigen_samples(samples.matrix, samples.num_rows,
			samples.num_cols);
	Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);
	eigen_samples=eigen_L*eigen_samples;

	/* add mean */
	Map<VectorXd> eigen_mean(m_mean.vector, m_mean.vlen);
	eigen_samples.colwise()+=eigen_mean;

	return samples;
}

SGVector<float64_t> CGaussianDistribution::log_pdf(SGMatrix<float64_t> samples) const
{
	REQUIRE(samples.num_cols>0, "Number of samples must be positive, but is %d\n",
			samples.num_cols);
	REQUIRE(samples.num_rows=m_dimension, "Sample dimension (%d) does not match"
			"Gaussian dimension (%d)\n", samples.num_rows, m_dimension);

	/* for easier to read code */
	index_t num_samples=samples.num_cols;

	float64_t const_part=-0.5 * m_dimension * CMath::log(2 * CMath::PI);

	/* determinant is product of diagonal elements of triangular matrix */
	float64_t log_det_part=0;
	Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);
	VectorXd diag=eigen_L.diagonal();
	log_det_part=-diag.array().log().sum();

	/* sample based part */
	Map<MatrixXd> eigen_samples(samples.matrix, samples.num_rows,
			samples.num_cols);
	Map<VectorXd> eigen_mean(m_mean.vector, m_mean.vlen);

	/* substract mean from samples (create copy) */
	SGMatrix<float64_t> centred(m_dimension, num_samples);
	Map<MatrixXd> eigen_centred(centred.matrix, centred.num_rows,
			centred.num_cols);
	for (index_t dim=0; dim<m_dimension; ++dim)
	{
		for (index_t sample_idx=0; sample_idx<num_samples; ++sample_idx)
			centred(dim,sample_idx)=samples(dim,sample_idx)-m_mean[dim];
	}

	/* solve the linear system based on factorization */
	MatrixXd solved=eigen_L.triangularView<Lower>().solve(eigen_centred);
	solved=eigen_L.transpose().triangularView<Upper>().solve(solved);

	/* one quadratic part x^T C^-1 x for each sample x */
	SGVector<float64_t> result(num_samples);
	Map<VectorXd> eigen_result(result.vector, result.vlen);
	for (index_t i=0; i<num_samples; ++i)
	{
		/* i-th centred sample */
		VectorXd left=eigen_centred.block(0, i, m_dimension, 1);

		/* inverted covariance times i-th centred sample */
		VectorXd right=solved.block(0,i,m_dimension,1);
		result[i]=-0.5*left.dot(right);
	}

	/* combine and return */
	eigen_result=eigen_result.array()+(log_det_part+const_part);

	/* contains everything */
	return result;

}

void CGaussianDistribution::init()
{
	SG_ADD(&m_mean, "mean", "Mean of the Gaussian.", MS_NOT_AVAILABLE);
	SG_ADD(&m_L, "L", "Lower factor of covariance matrix, "
			"depending on the factorization type.", MS_NOT_AVAILABLE);
}

#endif // HAVE_EIGEN3
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  Exception.cpp                           *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Frank C. Anderson                                               *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/* Note: This code was originally developed by Realistic Dynamics Inc.
 * Author: Frank C. Anderson
 */

// INCLUDES
#include <iostream>
#include <string>
#include <cassert>
#include "osimCommonDLL.h"
#include "Exception.h"
#include "IO.h"
#include "Object.h"

using namespace OpenSim;
using namespace std;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 *
 * @param aTrueFalse Scientific notation if true, and float if false.
 */
Exception::
Exception(const string &aMsg,const string &aFile,int aLine):
exception()
{
    setNull();

    setMessage(aMsg);
    _file = aFile;
    _line = aLine;

// make it assert false when debugging...
//#ifndef NDEBUG
//  print(cout);
//  assert(false);
//#endif
}

Exception::Exception(const std::string& file,
                     size_t line,
                     const std::string& func) {
    addMessage("\tIn file " + file + ":" + std::to_string(line) + "\n" +
               "\tIn function '" + func + "'");
}

Exception::Exception(const std::string& file,
                     size_t line,
                     const std::string& func,
                     const std::string& msg)
    : Exception{file, line, func} {
    addMessage(msg);
}

Exception::Exception(const std::string& file,
              size_t line,
              const std::string& func,
              const Object& obj)
    : Exception{file, line, func} {
    std::string className = obj.getConcreteClassName();
    std::string objName = obj.getName();
    if (objName.empty()) objName = "<no-name>";
    addMessage("\tIn object '" + objName + "' of type " + className + ".");
}

Exception::Exception(const std::string& file,
              size_t line,
              const std::string& func,
              const Object& obj,
                     const std::string& msg)
    : Exception{file, line, func, obj} {
    addMessage(msg);
}

void
Exception::addMessage(const std::string& msg) {
    if(_msg.length() == 0)
        _msg = msg;
    else
        _msg = msg + "\n" + _msg;
}

const char*
Exception::what() const noexcept {
    return getMessage();
}

//-----------------------------------------------------------------------------
// NULL
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Set the member variables to NULL values.
 */
void Exception::
setNull()
{
    setMessage("");
    _line = -1;
}

//=============================================================================
// GET AND SET
//=============================================================================
//-----------------------------------------------------------------------------
// MESSAGE
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Set the exception message.
 *
 * @param aMsg Message.
 */
void Exception::
setMessage(const string &aMsg)
{
    _msg = aMsg;
}
//_____________________________________________________________________________
/**
 * Get the exception message.
 *
 * @return Message.
 */
const char* Exception::
getMessage() const
{
    return(_msg.c_str());
}

//=============================================================================
// IO
//=============================================================================
//-----------------------------------------------------------------------------
// PRINT
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Print the exception to an output stream.
 *
 * @param aOut Output stream
 */
void Exception::
print(ostream &aOut) const
{
    // HEADER
    aOut << "\nException:\n";

    // MESSAGE
    // Account for the _msg being multiple lines -- we want to prepend two spaces before each new line
    string formattedMsg = IO::formatText(_msg, "  ", 75);
    aOut << "  " << formattedMsg << endl;

    // FILE
    if(_file.size()>0) {
        aOut << "  file= " << _file << '\n';
    }

    // LINE
    if(_line>=0) {
        aOut << "  line= " << _line << '\n';
    }

    // RETURN
    aOut << '\n' << endl;
}

<commit_msg>Exception message shows file name, not full path.<commit_after>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  Exception.cpp                           *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Frank C. Anderson                                               *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/* Note: This code was originally developed by Realistic Dynamics Inc.
 * Author: Frank C. Anderson
 */

// INCLUDES
#include <iostream>
#include <string>
#include <cassert>
#include "osimCommonDLL.h"
#include "Exception.h"
#include "IO.h"
#include "Object.h"

using namespace OpenSim;
using namespace std;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 *
 * @param aTrueFalse Scientific notation if true, and float if false.
 */
Exception::
Exception(const string &aMsg,const string &aFile,int aLine):
exception()
{
    setNull();

    setMessage(aMsg);
    _file = aFile;
    _line = aLine;

// make it assert false when debugging...
//#ifndef NDEBUG
//  print(cout);
//  assert(false);
//#endif
}

namespace {
/// Grab the last element of a file path.
std::string findFileName(const std::string& filepath) {
    // Based on a similar function from Simbody.
    std::string::size_type pos = filepath.find_last_of("/\\");
    if (pos + 1 >= filepath.size()) pos = 0;
    return filepath.substr(pos + 1);
}
} // namespace

Exception::Exception(const std::string& file,
                     size_t line,
                     const std::string& func) {
    addMessage("\tThrown at " + findFileName(file) + ":" +
            std::to_string(line) + " in " + func + "().");
}

Exception::Exception(const std::string& file,
                     size_t line,
                     const std::string& func,
                     const std::string& msg)
    : Exception{file, line, func} {
    addMessage(msg);
}

Exception::Exception(const std::string& file,
              size_t line,
              const std::string& func,
              const Object& obj)
    : Exception{file, line, func} {
    std::string className = obj.getConcreteClassName();
    std::string objName = obj.getName();
    if (objName.empty()) objName = "<no-name>";
    addMessage("\tIn Object '" + objName + "' of type " + className + ".");
}

Exception::Exception(const std::string& file,
              size_t line,
              const std::string& func,
              const Object& obj,
                     const std::string& msg)
    : Exception{file, line, func, obj} {
    addMessage(msg);
}

void
Exception::addMessage(const std::string& msg) {
    if(_msg.length() == 0)
        _msg = msg;
    else
        _msg = msg + "\n" + _msg;
}

const char*
Exception::what() const noexcept {
    return getMessage();
}

//-----------------------------------------------------------------------------
// NULL
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Set the member variables to NULL values.
 */
void Exception::
setNull()
{
    setMessage("");
    _line = -1;
}

//=============================================================================
// GET AND SET
//=============================================================================
//-----------------------------------------------------------------------------
// MESSAGE
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Set the exception message.
 *
 * @param aMsg Message.
 */
void Exception::
setMessage(const string &aMsg)
{
    _msg = aMsg;
}
//_____________________________________________________________________________
/**
 * Get the exception message.
 *
 * @return Message.
 */
const char* Exception::
getMessage() const
{
    return(_msg.c_str());
}

//=============================================================================
// IO
//=============================================================================
//-----------------------------------------------------------------------------
// PRINT
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Print the exception to an output stream.
 *
 * @param aOut Output stream
 */
void Exception::
print(ostream &aOut) const
{
    // HEADER
    aOut << "\nException:\n";

    // MESSAGE
    // Account for the _msg being multiple lines -- we want to prepend two spaces before each new line
    string formattedMsg = IO::formatText(_msg, "  ", 75);
    aOut << "  " << formattedMsg << endl;

    // FILE
    if(_file.size()>0) {
        aOut << "  file= " << _file << '\n';
    }

    // LINE
    if(_line>=0) {
        aOut << "  line= " << _line << '\n';
    }

    // RETURN
    aOut << '\n' << endl;
}

<|endoftext|>


<commit_before>/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org

Copyright (c) 2000-2013 Torus Knot Software Ltd
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
-----------------------------------------------------------------------------
*/
#include "SamplePlugin.h"
#include "VolumeTerrain.h"

#include "OgreVolumeTextureSource.h"
#include "OgreVolumeCSGSource.h"

#include "OgreRay.h"

using namespace Ogre;
using namespace OgreBites;
using namespace Ogre::Volume;

const Real Sample_VolumeTerrain::MOUSE_MODIFIER_TIME_LIMIT = (Real)0.033333;

void Sample_VolumeTerrain::setupContent(void)
{
    setupControls();

    // Skydome
    mSceneMgr->setSkyDome(true, "Examples/CloudySky", 5, 8);

    // Light
    Light* directionalLight0 = mSceneMgr->createLight("directionalLight0");
    directionalLight0->setType(Light::LT_DIRECTIONAL);
    directionalLight0->setDirection(Vector3((Real)1, (Real)-1, (Real)1));
    directionalLight0->setDiffuseColour((Real)1, (Real)0.98, (Real)0.73);
    directionalLight0->setSpecularColour((Real)0.1, (Real)0.1, (Real)0.1);

    // Volume
    mVolumeRoot = OGRE_NEW Chunk();
    mVolumeRootNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("VolumeParent");
    Timer t;
    mVolumeRoot->load(mVolumeRootNode, mSceneMgr, "volumeTerrain.cfg", true);
    LogManager::getSingleton().stream() << "Loaded volume terrain in " << t.getMillisecondsCPU() << " ms";

    // Camera
    mCamera->setPosition((Real)3264, (Real)2700, (Real)3264);
    mCamera->lookAt((Real)0, (Real)100, (Real)0);
    mCamera->setNearClipDistance((Real)0.5);

}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::setupControls(void)
{
    mMouseState = 0;
    mTrayMgr->showCursor();
#if OGRE_PLATFORM != OGRE_PLATFORM_APPLE_IOS
        setDragLook(true);
#endif
    mCameraMan->setTopSpeed((Real)100.0);
    // make room for the volume
    mTrayMgr->showLogo(TL_TOPRIGHT);
    mTrayMgr->showFrameStats(TL_TOPRIGHT);
    mTrayMgr->toggleAdvancedFrameStats();

    mTrayMgr->createTextBox(TL_TOPLEFT, "VolumeTerrainHelp", "Usage:\n\nHold the left mouse button, press\nwasd for movement and move the\nmouse for the direction.\nYou can add spheres with the\nmiddle mouse button and remove\nspheres with the right one.", 310, 150);
}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::cleanupContent(void)
{
    delete mVolumeRoot->getChunkParameters()->src;
    OGRE_DELETE mVolumeRoot;
    mVolumeRoot = 0;
}

//-----------------------------------------------------------------------

Sample_VolumeTerrain::Sample_VolumeTerrain(void) : mVolumeRoot(0), mHideAll(false)
{
    mInfo["Title"] = "Volume Terrain";
    mInfo["Description"] = "Demonstrates a volumetric terrain defined by an 3D texture and manipulation of the volume. The middle mouse button adds a sphere, a rightclick removes one.";
    mInfo["Thumbnail"] = "thumb_volumeterrain.png";
    mInfo["Category"] = "Geometry";
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::keyPressed(const OIS::KeyEvent& evt)
{
    if (evt.key == OIS::KC_F10)
    {
        mVolumeRoot->setVolumeVisible(!mVolumeRoot->getVolumeVisible());
    }
    if (evt.key == OIS::KC_F11)
    {
        mVolumeRoot->setOctreeVisible(!mVolumeRoot->getOctreeVisible());
    }
    if (evt.key == OIS::KC_F12)
    {
        mVolumeRoot->setDualGridVisible(!mVolumeRoot->getDualGridVisible());
    }
    if (evt.key == OIS::KC_H)
    {
        if (mHideAll)
        {
            mTrayMgr->showAll();
        }
        else
        {
            mTrayMgr->hideAll();
        }
        mHideAll = !mHideAll;
    }
    return SdkSample::keyPressed(evt);
}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::shootRay(Ray ray, bool doUnion)
{
    Vector3 intersection;
    Real scale = mVolumeRoot->getChunkParameters()->scale;
    bool intersects = mVolumeRoot->getChunkParameters()->src->getFirstRayIntersection(ray, intersection, scale);
    if (intersects)
    {
        Real radius = (Real)2.5;
        CSGSphereSource sphere(radius, intersection);
        // ? : doesn't work here somehow?
        CSGOperationSource *operation;
        if (doUnion)
        {
            operation = new CSGUnionSource();
        }
        else
        {
            operation = new CSGDifferenceSource();
        }
        static_cast<TextureSource*>(mVolumeRoot->getChunkParameters()->src)->combineWithSource(operation, &sphere, intersection, radius * (Real)2.0);

        mVolumeRoot->getChunkParameters()->updateFrom = intersection - radius * (Real)2.0;
        mVolumeRoot->getChunkParameters()->updateTo = intersection + radius * (Real)2.0;
        mVolumeRoot->load(mVolumeRootNode, Vector3::ZERO, Vector3(384), 5, mVolumeRoot->getChunkParameters());
        delete operation;
    }
}

//-----------------------------------------------------------------------

#if (OGRE_PLATFORM == OGRE_PLATFORM_APPLE_IOS) || (OGRE_PLATFORM == OGRE_PLATFORM_ANDROID)
bool Sample_VolumeTerrain::touchPressed(const OIS::MultiTouchEvent& evt)
{
    Real x = (Real)evt.state.X.abs / (Real)evt.state.width;
    Real y = (Real)evt.state.Y.abs / (Real)evt.state.height;
    Ray ray = mCamera->getCameraToViewportRay(x, y);
    shootRay(ray, true);

    return SdkSample::touchPressed(evt);
}

#else

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mousePressed(const OIS::MouseEvent& evt, OIS::MouseButtonID id)
{
    if (mMouseState == 0)
    {
        if (id == OIS::MB_Middle)
        {
            mMouseState = 1;
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
        }
        if (id == OIS::MB_Right)
        {
            mMouseState = 2;
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
        }
    }

    return SdkSample::mousePressed(evt, id);
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mouseReleased(const OIS::MouseEvent& evt, OIS::MouseButtonID id)
{

    if (id == OIS::MB_Middle || id == OIS::MB_Right)
    {
        mMouseState = 0;
    }

    return SdkSample::mouseReleased(evt, id);
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mouseMoved(const OIS::MouseEvent& evt)
{
    mMouseX = (Real)evt.state.X.abs / (Real)evt.state.width;
    mMouseY = (Real)evt.state.Y.abs / (Real)evt.state.height;
    return SdkSample::mouseMoved(evt);
}

#endif

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
    if (mMouseState != 0)
    {
        mMouseCountdown -= evt.timeSinceLastEvent;
        if (mMouseCountdown <= (Real)0.0)
        {
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
            Ray ray = mCamera->getCameraToViewportRay(mMouseX, mMouseY);
            shootRay(ray, mMouseState == 1);
        }
    }
    return SdkSample::frameRenderingQueued(evt);
}

//-----------------------------------------------------------------------

#ifndef OGRE_STATIC_LIB

SamplePlugin* sp;
Sample* s;

//-----------------------------------------------------------------------

extern "C" _OgreSampleExport void dllStartPlugin()
{
    s = new Sample_VolumeTerrain();
    sp = OGRE_NEW SamplePlugin(s->getInfo()["Title"] + " Sample");
    sp->addSample(s);
    Root::getSingleton().installPlugin(sp);
}

//-----------------------------------------------------------------------

extern "C" _OgreSampleExport void dllStopPlugin()
{
    Root::getSingleton().uninstallPlugin(sp);
    OGRE_DELETE sp;
    delete s;
}

#endif
<commit_msg>Volume Rendering: small code cleanup<commit_after>/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org

Copyright (c) 2000-2013 Torus Knot Software Ltd
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
-----------------------------------------------------------------------------
*/
#include "SamplePlugin.h"
#include "VolumeTerrain.h"

#include "OgreVolumeTextureSource.h"
#include "OgreVolumeCSGSource.h"

#include "OgreRay.h"

using namespace Ogre;
using namespace OgreBites;
using namespace Ogre::Volume;

const Real Sample_VolumeTerrain::MOUSE_MODIFIER_TIME_LIMIT = (Real)0.033333;

void Sample_VolumeTerrain::setupContent(void)
{
    setupControls();

    // Skydome
    mSceneMgr->setSkyDome(true, "Examples/CloudySky", 5, 8);

    // Light
    Light* directionalLight0 = mSceneMgr->createLight("directionalLight0");
    directionalLight0->setType(Light::LT_DIRECTIONAL);
    directionalLight0->setDirection(Vector3((Real)1, (Real)-1, (Real)1));
    directionalLight0->setDiffuseColour((Real)1, (Real)0.98, (Real)0.73);
    directionalLight0->setSpecularColour((Real)0.1, (Real)0.1, (Real)0.1);

    // Volume
    mVolumeRoot = OGRE_NEW Chunk();
    mVolumeRootNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("VolumeParent");
    Timer t;
    mVolumeRoot->load(mVolumeRootNode, mSceneMgr, "volumeTerrain.cfg", true);
    LogManager::getSingleton().stream() << "Loaded volume terrain in " << t.getMillisecondsCPU() << " ms";

    // Camera
    mCamera->setPosition((Real)3264, (Real)2700, (Real)3264);
    mCamera->lookAt((Real)0, (Real)100, (Real)0);
    mCamera->setNearClipDistance((Real)0.5);

}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::setupControls(void)
{
    mMouseState = 0;
    mTrayMgr->showCursor();
#if OGRE_PLATFORM != OGRE_PLATFORM_APPLE_IOS
        setDragLook(true);
#endif
    mCameraMan->setTopSpeed((Real)100.0);
    // make room for the volume
    mTrayMgr->showLogo(TL_TOPRIGHT);
    mTrayMgr->showFrameStats(TL_TOPRIGHT);
    mTrayMgr->toggleAdvancedFrameStats();

    mTrayMgr->createTextBox(TL_TOPLEFT, "VolumeTerrainHelp", "Usage:\n\nHold the left mouse button, press\nwasd for movement and move the\nmouse for the direction.\nYou can add spheres with the\nmiddle mouse button and remove\nspheres with the right one.", 310, 150);
}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::cleanupContent(void)
{
    delete mVolumeRoot->getChunkParameters()->src;
    OGRE_DELETE mVolumeRoot;
    mVolumeRoot = 0;
}

//-----------------------------------------------------------------------

Sample_VolumeTerrain::Sample_VolumeTerrain(void) : mVolumeRoot(0), mHideAll(false)
{
    mInfo["Title"] = "Volume Terrain";
    mInfo["Description"] = "Demonstrates a volumetric terrain defined by an 3D texture and manipulation of the volume. The middle mouse button adds a sphere, a rightclick removes one.";
    mInfo["Thumbnail"] = "thumb_volumeterrain.png";
    mInfo["Category"] = "Geometry";
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::keyPressed(const OIS::KeyEvent& evt)
{
    if (evt.key == OIS::KC_F10)
    {
        mVolumeRoot->setVolumeVisible(!mVolumeRoot->getVolumeVisible());
    }
    if (evt.key == OIS::KC_F11)
    {
        mVolumeRoot->setOctreeVisible(!mVolumeRoot->getOctreeVisible());
    }
    if (evt.key == OIS::KC_F12)
    {
        mVolumeRoot->setDualGridVisible(!mVolumeRoot->getDualGridVisible());
    }
    if (evt.key == OIS::KC_H)
    {
        if (mHideAll)
        {
            mTrayMgr->showAll();
        }
        else
        {
            mTrayMgr->hideAll();
        }
        mHideAll = !mHideAll;
    }
    return SdkSample::keyPressed(evt);
}

//-----------------------------------------------------------------------

void Sample_VolumeTerrain::shootRay(Ray ray, bool doUnion)
{
    Vector3 intersection;
    Real scale = mVolumeRoot->getChunkParameters()->scale;
    bool intersects = mVolumeRoot->getChunkParameters()->src->getFirstRayIntersection(ray, intersection, scale);
    if (intersects)
    {
        Real radius = (Real)2.5;
        CSGSphereSource sphere(radius, intersection);
        CSGOperationSource *operation = doUnion ? reinterpret_cast<CSGOperationSource*>(new CSGUnionSource()) : new CSGDifferenceSource();
        static_cast<TextureSource*>(mVolumeRoot->getChunkParameters()->src)->combineWithSource(operation, &sphere, intersection, radius * (Real)2.0);

        mVolumeRoot->getChunkParameters()->updateFrom = intersection - radius * (Real)2.0;
        mVolumeRoot->getChunkParameters()->updateTo = intersection + radius * (Real)2.0;
        mVolumeRoot->load(mVolumeRootNode, Vector3::ZERO, Vector3(384), 5, mVolumeRoot->getChunkParameters());
        delete operation;
    }
}

//-----------------------------------------------------------------------

#if (OGRE_PLATFORM == OGRE_PLATFORM_APPLE_IOS) || (OGRE_PLATFORM == OGRE_PLATFORM_ANDROID)
bool Sample_VolumeTerrain::touchPressed(const OIS::MultiTouchEvent& evt)
{
    Real x = (Real)evt.state.X.abs / (Real)evt.state.width;
    Real y = (Real)evt.state.Y.abs / (Real)evt.state.height;
    Ray ray = mCamera->getCameraToViewportRay(x, y);
    shootRay(ray, true);

    return SdkSample::touchPressed(evt);
}

#else

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mousePressed(const OIS::MouseEvent& evt, OIS::MouseButtonID id)
{
    if (mMouseState == 0)
    {
        if (id == OIS::MB_Middle)
        {
            mMouseState = 1;
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
        }
        if (id == OIS::MB_Right)
        {
            mMouseState = 2;
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
        }
    }

    return SdkSample::mousePressed(evt, id);
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mouseReleased(const OIS::MouseEvent& evt, OIS::MouseButtonID id)
{

    if (id == OIS::MB_Middle || id == OIS::MB_Right)
    {
        mMouseState = 0;
    }

    return SdkSample::mouseReleased(evt, id);
}

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::mouseMoved(const OIS::MouseEvent& evt)
{
    mMouseX = (Real)evt.state.X.abs / (Real)evt.state.width;
    mMouseY = (Real)evt.state.Y.abs / (Real)evt.state.height;
    return SdkSample::mouseMoved(evt);
}

#endif

//-----------------------------------------------------------------------

bool Sample_VolumeTerrain::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
    if (mMouseState != 0)
    {
        mMouseCountdown -= evt.timeSinceLastEvent;
        if (mMouseCountdown <= (Real)0.0)
        {
            mMouseCountdown = MOUSE_MODIFIER_TIME_LIMIT;
            Ray ray = mCamera->getCameraToViewportRay(mMouseX, mMouseY);
            shootRay(ray, mMouseState == 1);
        }
    }
    return SdkSample::frameRenderingQueued(evt);
}

//-----------------------------------------------------------------------

#ifndef OGRE_STATIC_LIB

SamplePlugin* sp;
Sample* s;

//-----------------------------------------------------------------------

extern "C" _OgreSampleExport void dllStartPlugin()
{
    s = new Sample_VolumeTerrain();
    sp = OGRE_NEW SamplePlugin(s->getInfo()["Title"] + " Sample");
    sp->addSample(s);
    Root::getSingleton().installPlugin(sp);
}

//-----------------------------------------------------------------------

extern "C" _OgreSampleExport void dllStopPlugin()
{
    Root::getSingleton().uninstallPlugin(sp);
    OGRE_DELETE sp;
    delete s;
}

#endif
<|endoftext|>


<commit_before>#define CATCH_CONFIG_RUNNER

#include "videosourcefactory.h"
#include "catch.hpp"

gg::Device device;
gg::ColourSpace colour, other_colour;

int main(int argc, char * argv[])
{
    bool args_ok = true;
    if (argc < 3)
        args_ok = false;
    else
    {
        if (strcmp(argv[1], "DVI") == 0)
            device = gg::DVI2PCIeDuo_DVI;
        else if (strcmp(argv[1], "SDI") == 0)
            device = gg::DVI2PCIeDuo_SDI;
        else
            args_ok = false;
        if (strcmp(argv[2], "BGRA") == 0)
        {
            colour = gg::BGRA;
            other_colour = gg::I420;
        }
        else if (strcmp(argv[2], "I420") == 0)
        {
            colour = gg::I420;
            other_colour = gg::BGRA;
        }
        else
            args_ok = false;
    }
    if (not args_ok)
    {
        printf("Synopsis: %s DVI|SDI BGRA|I420\n", argv[0]);
        return EXIT_FAILURE;
    }
    return Catch::Session().run();
}

TEST_CASE( "get_instance returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    gg::VideoSourceFactory & factory_ = gg::VideoSourceFactory::get_instance();
    REQUIRE( &factory == &factory_ );
}

TEST_CASE( "get_device returns connection", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = nullptr;
    source = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source->get_colour() == colour );

    gg::VideoFrame frame(colour, false);
    REQUIRE( frame.cols() == 0 );
    REQUIRE( frame.rows() == 0 );
    REQUIRE( source->get_frame(frame) );
    REQUIRE( frame.cols() > 0 );
    REQUIRE( frame.rows() > 0 );
}

TEST_CASE( "get_device returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_ = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source == source_ );
}

#if defined(USE_LIBVLC) or defined(USE_EPIPHANSDK)
#if defined(USE_OPENCV)
TEST_CASE( "get_device does not create duplicate", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();

    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_duplicate = nullptr;
    REQUIRE_THROWS_AS(
        source_duplicate = factory.get_device(device, other_colour),
        gg::DeviceAlreadyConnected
    );
    REQUIRE( source_duplicate == nullptr );
}

TEST_CASE( "free_device garbage-collects device", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    IVideoSource * source = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );

    gg::VideoFrame frame(colour, false);
    REQUIRE( frame.cols() == 0 );
    REQUIRE( frame.rows() == 0 );
    REQUIRE( source->get_frame(frame) );
    REQUIRE( frame.cols() > 0 );
    REQUIRE( frame.rows() > 0 );

    factory.free_device(device);
    source = nullptr;
    source = factory.get_device(device, other_colour);
    REQUIRE_FALSE( source == nullptr );
}
#endif
#endif
<commit_msg>Issue #121: added documentation to tests protected by macro checks related to colour space support<commit_after>#define CATCH_CONFIG_RUNNER

#include "videosourcefactory.h"
#include "catch.hpp"

gg::Device device;
gg::ColourSpace colour, other_colour;

int main(int argc, char * argv[])
{
    bool args_ok = true;
    if (argc < 3)
        args_ok = false;
    else
    {
        if (strcmp(argv[1], "DVI") == 0)
            device = gg::DVI2PCIeDuo_DVI;
        else if (strcmp(argv[1], "SDI") == 0)
            device = gg::DVI2PCIeDuo_SDI;
        else
            args_ok = false;
        if (strcmp(argv[2], "BGRA") == 0)
        {
            colour = gg::BGRA;
            other_colour = gg::I420;
        }
        else if (strcmp(argv[2], "I420") == 0)
        {
            colour = gg::I420;
            other_colour = gg::BGRA;
        }
        else
            args_ok = false;
    }
    if (not args_ok)
    {
        printf("Synopsis: %s DVI|SDI BGRA|I420\n", argv[0]);
        return EXIT_FAILURE;
    }
    return Catch::Session().run();
}

TEST_CASE( "get_instance returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    gg::VideoSourceFactory & factory_ = gg::VideoSourceFactory::get_instance();
    REQUIRE( &factory == &factory_ );
}

TEST_CASE( "get_device returns connection", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = nullptr;
    source = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source->get_colour() == colour );

    gg::VideoFrame frame(colour, false);
    REQUIRE( frame.cols() == 0 );
    REQUIRE( frame.rows() == 0 );
    REQUIRE( source->get_frame(frame) );
    REQUIRE( frame.cols() > 0 );
    REQUIRE( frame.rows() > 0 );
}

TEST_CASE( "get_device returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_ = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source == source_ );
}

/* The following tests are protected with these macro
 * definition checks because they involve support using
 * both colour spaces.
 */
#if defined(USE_OPENCV)                            // BGRA colour space
#if defined(USE_LIBVLC) or defined(USE_EPIPHANSDK) // I420 colour space
TEST_CASE( "get_device does not create duplicate", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();

    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_duplicate = nullptr;
    REQUIRE_THROWS_AS(
        source_duplicate = factory.get_device(device, other_colour),
        gg::DeviceAlreadyConnected
    );
    REQUIRE( source_duplicate == nullptr );
}

TEST_CASE( "free_device garbage-collects device", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    IVideoSource * source = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );

    gg::VideoFrame frame(colour, false);
    REQUIRE( frame.cols() == 0 );
    REQUIRE( frame.rows() == 0 );
    REQUIRE( source->get_frame(frame) );
    REQUIRE( frame.cols() > 0 );
    REQUIRE( frame.rows() > 0 );

    factory.free_device(device);
    source = nullptr;
    source = factory.get_device(device, other_colour);
    REQUIRE_FALSE( source == nullptr );
}
#endif
#endif
<|endoftext|>


<commit_before>/**
* @file MonteCarloSelfLocatorSimple.cpp
*
* @author Heinrich Mellmann
* Implementation of class MonteCarloSelfLocatorSimple
*/

#include "MonteCarloSelfLocatorSimple.h"

// debug
#include "Tools/Debug/DebugBufferedOutput.h"
#include "Tools/Debug/DebugRequest.h"

// tools
#include "Tools/Math/Probabilistics.h"
#include "Tools.h"

using namespace std;
using namespace mcsl;

MonteCarloSelfLocatorSimple::MonteCarloSelfLocatorSimple()
  :
  canopyClustering(parameters.thresholdCanopy)
{
  // debug
  DEBUG_REQUEST_REGISTER("MCSLS:reset_samples", "reset the sample set", false);

  // fiald drawings
  DEBUG_REQUEST_REGISTER("MCSLS:draw_Samples", "draw sample set before resampling", false);
  DEBUG_REQUEST_REGISTER("MCSLS:draw_post_choice", "", false);
  DEBUG_REQUEST_REGISTER("MCSLS:draw_sensor_belief", "", false);

  initializeSampleSet(getFieldInfo().carpetRect, theSampleSet);
}

void MonteCarloSelfLocatorSimple::execute()
{

  DEBUG_REQUEST("MCSLS:reset_samples",
    initializeSampleSet(getFieldInfo().carpetRect, theSampleSet);

    DEBUG_REQUEST("MCSLS:draw_Samples",
      theSampleSet.drawImportance();
    );
    return;
  );

  updateByOdometry(theSampleSet, parameters.motionNoise);
  lastRobotOdometry = getOdometryData();

  theSampleSet.resetLikelihood();
  updateBySensors(theSampleSet);
  //resampleSimple(theSampleSet);
  resampleGT07(theSampleSet, true);

  DEBUG_REQUEST("MCSLS:draw_Samples",
    theSampleSet.drawImportance();
  );

  DEBUG_REQUEST("MCSLS:draw_sensor_belief",
    draw_sensor_belief();
  );
}//end execute

void MonteCarloSelfLocatorSimple::updateByOdometry(SampleSet& sampleSet, bool noise) const
{
  Pose2D odometryDelta = getOdometryData() - lastRobotOdometry;
  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    sampleSet[i] += odometryDelta;

    if(noise) {
      applySimpleNoise(sampleSet[i], parameters.motionNoiseDistance, parameters.motionNoiseAngle);
    }
  }
}//end updateByOdometry

bool MonteCarloSelfLocatorSimple::updateBySensors(SampleSet& sampleSet) const
{
  if(parameters.updateByGoalPost)
  {
    if(getGoalPercept().getNumberOfSeenPosts() > 0) {
      updateByGoalPosts(getGoalPercept(), sampleSet);
    }
    if(getGoalPerceptTop().getNumberOfSeenPosts() > 0) {
      updateByGoalPosts(getGoalPerceptTop(), sampleSet);
    }
  }//end update by goal posts

  return true;
}

void MonteCarloSelfLocatorSimple::updateByGoalPosts(const GoalPercept& goalPercept, SampleSet& sampleSet) const
{
  for(int i = 0; i < goalPercept.getNumberOfSeenPosts(); i++)
  {
    const GoalPercept::GoalPost& seenPost = goalPercept.getPost(i);
    updateBySingleGoalPost(seenPost, sampleSet);
  }
}

void MonteCarloSelfLocatorSimple::updateBySingleGoalPost(const GoalPercept::GoalPost& seenPost, SampleSet& sampleSet) const
{
  const double sigmaAngle    = parameters.goalPostSigmaAngle;
  const double sigmaDistance = parameters.goalPostSigmaDistance;
  const double cameraHeight  = getCameraMatrix().translation.z;

  const double seenDistance = seenPost.position.abs();
  const double seenAngle = seenPost.position.angle();

  const Vector2d* leftGoalPosition = NULL;
  const Vector2d* rightGoalPosition = NULL;

  for (unsigned int j = 0; j < sampleSet.size(); j++)
  {
    Sample& sample = sampleSet[j];
    Vector2d expectedPostPosition;

    // each particle decides for itself
    if(fabs(Math::normalize(sample.rotation + seenAngle)) < Math::pi_2)
    {
      leftGoalPosition = &(getFieldInfo().opponentGoalPostLeft);
      rightGoalPosition = &(getFieldInfo().opponentGoalPostRight);
    } else {
      // own goals are switched (!)
      leftGoalPosition = &(getFieldInfo().ownGoalPostRight);
      rightGoalPosition = &(getFieldInfo().ownGoalPostLeft);
    }

    if(seenPost.type == GoalPercept::GoalPost::rightPost)
    {
      expectedPostPosition = *rightGoalPosition;
      // switch if the robot is behind the goal
      if( (expectedPostPosition.x < 0 && sample.translation.x < expectedPostPosition.x) ||
          (expectedPostPosition.x > 0 && sample.translation.x > expectedPostPosition.x))
      {
        expectedPostPosition = *leftGoalPosition;
      }
    } else if(seenPost.type == GoalPercept::GoalPost::leftPost) {
      expectedPostPosition = *leftGoalPosition;
      // switch if the robot is behind the goal
      if( (expectedPostPosition.x < 0 && sample.translation.x < expectedPostPosition.x) ||
          (expectedPostPosition.x > 0 && sample.translation.x > expectedPostPosition.x))
      {
        expectedPostPosition = *rightGoalPosition;
      }
    } else { // unknown post
      Vector2d globalPercept = sample * seenPost.position;
      // choose the closest one
      if((globalPercept - *rightGoalPosition).abs() < (globalPercept - *leftGoalPosition).abs()) {
        expectedPostPosition = *rightGoalPosition;
      } else {
        expectedPostPosition = *leftGoalPosition;
      }
    }

    Vector2d relPost = sample/expectedPostPosition;
    double expectedDistance = relPost.abs();
    double expectedAngle = relPost.angle();

    DEBUG_REQUEST("MCSLS:draw_post_choice",
      PEN("00000099",1);
      LINE(sample.translation.x, sample.translation.y, expectedPostPosition.x, expectedPostPosition.y);
    );

    if(seenPost.positionReliable) {
      sample.likelihood *= computeDistanceWeight(seenDistance, expectedDistance, cameraHeight, sigmaDistance);
    }
    sample.likelihood *= computeAngleWeight(seenAngle, expectedAngle, sigmaAngle);

  } //end for j (samples)
}

void MonteCarloSelfLocatorSimple::resampleSimple(SampleSet& sampleSet, int number) const
{
  //sampleSet.normalize();
  sampleSet.sort();
  for(int i = 0; i < number; i++) {
    createRandomSample(getFieldInfo().carpetRect, sampleSet[i]);
  }
}

void MonteCarloSelfLocatorSimple::resampleGT07(SampleSet& sampleSet, bool noise) const
{
  int numberOfPartiklesToResample = 0;

  SampleSet oldSampleSet = sampleSet;
  oldSampleSet.normalize(parameters.resamplingThreshhold);
  //oldSampleSet.sort();

  double sum = -Math::random();
  unsigned int count = 0;

  unsigned int m = 0;  // Zaehler durchs Ausgangs-Set
  unsigned int n = 0;  // Zaehler durchs Ziel-Set

  for(m = 0; m < sampleSet.size(); m++)
  {
    sum += oldSampleSet[m].likelihood * oldSampleSet.size();

    // select the particle to copy
    while(count < sum && count < oldSampleSet.size())
    {
      if (n >= oldSampleSet.size() - numberOfPartiklesToResample) break;

      // copy the selected particle
      sampleSet[n] = oldSampleSet[m];
      if(noise) {
        applySimpleNoise(sampleSet[n], parameters.processNoiseDistance, parameters.processNoiseAngle);
      }

      n++;
      count++;
    }//end while

    if (n >= oldSampleSet.size()-numberOfPartiklesToResample) break;
  }//end for

  // generate some random samples
  for(; n < oldSampleSet.size(); n++) {
    createRandomSample(getFieldInfo().carpetRect, sampleSet[n]);
  }

  // fill up by copying random samples (shouldn't happen)
  while (n < sampleSet.size())
  {
    int i = Math::random(sampleSet.size());
    sampleSet[n++] = sampleSet[i];
  }
}

void MonteCarloSelfLocatorSimple::draw_sensor_belief() const
{
  static const int ySize = 20;
  static const int xSize = 30;
  double yWidth = 0.5*getFieldInfo().yFieldLength/(double)ySize;
  double xWidth = 0.5*getFieldInfo().xFieldLength/(double)xSize;

  FIELD_DRAWING_CONTEXT;
  DebugDrawings::Color white(1.0,1.0,1.0,0.0); // transparent
  DebugDrawings::Color black(0.0,0.0,0.0,1.0);

  double fixedRotation = theSampleSet.getMostLikelySample().rotation;

  // create new sample set
  SampleSet sampleSet(xSize*ySize);
  int idx = 0;

  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));
      sampleSet.samples[idx].translation = point;
      sampleSet.samples[idx].rotation = fixedRotation;
      sampleSet.samples[idx].likelihood = 1.0;
      idx++;
    }//end for
  }//end for

  updateBySensors(sampleSet);

  double maxValue = 0;
  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      maxValue = max(maxValue, sampleSet.samples[idx++].likelihood);
    }//end for
  }//end for

  if(maxValue == 0) return;

  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2<double> point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));

      double t = sampleSet.samples[idx++].likelihood / maxValue;
      DebugDrawings::Color color = black*t + white*(1-t);
      PEN(color, 20);
      FILLBOX(point.x - xWidth, point.y - yWidth, point.x+xWidth, point.y+yWidth);
    }//end for
  }//end for
}//end draw_closest_points<commit_msg>some cleanup<commit_after>/**
* @file MonteCarloSelfLocatorSimple.cpp
*
* @author Heinrich Mellmann
* Implementation of class MonteCarloSelfLocatorSimple
*/

#include "MonteCarloSelfLocatorSimple.h"

// debug
#include "Tools/Debug/DebugBufferedOutput.h"
#include "Tools/Debug/DebugRequest.h"

// tools
#include "Tools/Math/Probabilistics.h"
#include "Tools.h"

using namespace std;
using namespace mcsl;

MonteCarloSelfLocatorSimple::MonteCarloSelfLocatorSimple()
  :
  canopyClustering(parameters.thresholdCanopy)
{
  // debug
  DEBUG_REQUEST_REGISTER("MCSLS:reset_samples", "reset the sample set", false);

  // fiald drawings
  DEBUG_REQUEST_REGISTER("MCSLS:draw_Samples", "draw sample set before resampling", false);
  DEBUG_REQUEST_REGISTER("MCSLS:draw_post_choice", "", false);
  DEBUG_REQUEST_REGISTER("MCSLS:draw_sensor_belief", "", false);

  DEBUG_REQUEST_REGISTER("MCSLS:draw_resample_simple", "", false);
  DEBUG_REQUEST_REGISTER("MCSLS:draw_resample_gt", "", false);

  initializeSampleSet(getFieldInfo().carpetRect, theSampleSet);
}

void MonteCarloSelfLocatorSimple::execute()
{

  DEBUG_REQUEST("MCSLS:reset_samples",
    initializeSampleSet(getFieldInfo().carpetRect, theSampleSet);

    DEBUG_REQUEST("MCSLS:draw_Samples",
      theSampleSet.drawImportance();
    );
    return;
  );

  updateByOdometry(theSampleSet, parameters.motionNoise);
  lastRobotOdometry = getOdometryData();

  theSampleSet.resetLikelihood();
  updateBySensors(theSampleSet);
  //resampleSimple(theSampleSet);

  DEBUG_REQUEST("MCSLS:draw_resample_gt",
    resampleGT07(theSampleSet, true);
  );

  DEBUG_REQUEST("MCSLS:draw_resample_simple",
    resampleSimple(theSampleSet, 0);
  );

  DEBUG_REQUEST("MCSLS:draw_Samples",
    theSampleSet.drawImportance();
  );

  DEBUG_REQUEST("MCSLS:draw_sensor_belief",
    draw_sensor_belief();
  );
}//end execute

void MonteCarloSelfLocatorSimple::updateByOdometry(SampleSet& sampleSet, bool noise) const
{
  Pose2D odometryDelta = getOdometryData() - lastRobotOdometry;
  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    sampleSet[i] += odometryDelta;

    if(noise) {
      applySimpleNoise(sampleSet[i], parameters.motionNoiseDistance, parameters.motionNoiseAngle);
    }
  }
}//end updateByOdometry

bool MonteCarloSelfLocatorSimple::updateBySensors(SampleSet& sampleSet) const
{
  if(parameters.updateByGoalPost)
  {
    if(getGoalPercept().getNumberOfSeenPosts() > 0) {
      updateByGoalPosts(getGoalPercept(), sampleSet);
    }
    if(getGoalPerceptTop().getNumberOfSeenPosts() > 0) {
      updateByGoalPosts(getGoalPerceptTop(), sampleSet);
    }
  }//end update by goal posts

  return true;
}

void MonteCarloSelfLocatorSimple::updateByGoalPosts(const GoalPercept& goalPercept, SampleSet& sampleSet) const
{
  for(int i = 0; i < goalPercept.getNumberOfSeenPosts(); i++)
  {
    const GoalPercept::GoalPost& seenPost = goalPercept.getPost(i);
    updateBySingleGoalPost(seenPost, sampleSet);
  }
}

void MonteCarloSelfLocatorSimple::updateBySingleGoalPost(const GoalPercept::GoalPost& seenPost, SampleSet& sampleSet) const
{
  const double sigmaAngle    = parameters.goalPostSigmaAngle;
  const double sigmaDistance = parameters.goalPostSigmaDistance;
  const double cameraHeight  = getCameraMatrix().translation.z;

  const double seenDistance = seenPost.position.abs();
  const double seenAngle = seenPost.position.angle();

  const Vector2d* leftGoalPosition = NULL;
  const Vector2d* rightGoalPosition = NULL;

  for (unsigned int j = 0; j < sampleSet.size(); j++)
  {
    Sample& sample = sampleSet[j];
    Vector2d expectedPostPosition;

    // each particle decides for itself
    if(fabs(Math::normalize(sample.rotation + seenAngle)) < Math::pi_2)
    {
      leftGoalPosition = &(getFieldInfo().opponentGoalPostLeft);
      rightGoalPosition = &(getFieldInfo().opponentGoalPostRight);
    } else {
      // own goals are switched (!)
      leftGoalPosition = &(getFieldInfo().ownGoalPostRight);
      rightGoalPosition = &(getFieldInfo().ownGoalPostLeft);
    }

    if(seenPost.type == GoalPercept::GoalPost::rightPost)
    {
      expectedPostPosition = *rightGoalPosition;
      // switch if the robot is behind the goal
      if( (expectedPostPosition.x < 0 && sample.translation.x < expectedPostPosition.x) ||
          (expectedPostPosition.x > 0 && sample.translation.x > expectedPostPosition.x))
      {
        expectedPostPosition = *leftGoalPosition;
      }
    } else if(seenPost.type == GoalPercept::GoalPost::leftPost) {
      expectedPostPosition = *leftGoalPosition;
      // switch if the robot is behind the goal
      if( (expectedPostPosition.x < 0 && sample.translation.x < expectedPostPosition.x) ||
          (expectedPostPosition.x > 0 && sample.translation.x > expectedPostPosition.x))
      {
        expectedPostPosition = *rightGoalPosition;
      }
    } else { // unknown post
      Vector2d globalPercept = sample * seenPost.position;
      // choose the closest one
      if((globalPercept - *rightGoalPosition).abs() < (globalPercept - *leftGoalPosition).abs()) {
        expectedPostPosition = *rightGoalPosition;
      } else {
        expectedPostPosition = *leftGoalPosition;
      }
    }

    Vector2d relPost = sample/expectedPostPosition;
    double expectedDistance = relPost.abs();
    double expectedAngle = relPost.angle();

    DEBUG_REQUEST("MCSLS:draw_post_choice",
      PEN("00000099",1);
      LINE(sample.translation.x, sample.translation.y, expectedPostPosition.x, expectedPostPosition.y);
    );

    if(seenPost.positionReliable) {
      sample.likelihood *= computeDistanceWeight(seenDistance, expectedDistance, cameraHeight, sigmaDistance);
    }
    sample.likelihood *= computeAngleWeight(seenAngle, expectedAngle, sigmaAngle);

  } //end for j (samples)
}

void MonteCarloSelfLocatorSimple::resampleSimple(SampleSet& sampleSet, int number) const
{
  sampleSet.normalize();
  for(unsigned int i = 0; i < sampleSet.size(); i++) {
    if(sampleSet[i].likelihood < 0.01) {
      createRandomSample(getFieldInfo().carpetRect, sampleSet[i]);
    }
  }
}

void MonteCarloSelfLocatorSimple::resampleGT07(SampleSet& sampleSet, bool noise) const
{
  int numberOfPartiklesToResample = 0;

  SampleSet oldSampleSet = sampleSet;
  oldSampleSet.normalize(parameters.resamplingThreshhold);
  //oldSampleSet.sort();

  double sum = -Math::random();
  unsigned int count = 0;

  unsigned int m = 0;  // Zaehler durchs Ausgangs-Set
  unsigned int n = 0;  // Zaehler durchs Ziel-Set

  for(m = 0; m < sampleSet.size(); m++)
  {
    sum += oldSampleSet[m].likelihood * oldSampleSet.size();

    // select the particle to copy
    while(count < sum && count < oldSampleSet.size())
    {
      if (n >= oldSampleSet.size() - numberOfPartiklesToResample) break;

      // copy the selected particle
      sampleSet[n] = oldSampleSet[m];
      if(noise) {
        applySimpleNoise(sampleSet[n], parameters.processNoiseDistance, parameters.processNoiseAngle);
      }

      n++;
      count++;
    }//end while

    if (n >= oldSampleSet.size()-numberOfPartiklesToResample) break;
  }//end for

  // generate some random samples
  for(; n < oldSampleSet.size(); n++) {
    createRandomSample(getFieldInfo().carpetRect, sampleSet[n]);
  }

  // fill up by copying random samples (shouldn't happen)
  while (n < sampleSet.size())
  {
    int i = Math::random(sampleSet.size());
    sampleSet[n++] = sampleSet[i];
  }
}

void MonteCarloSelfLocatorSimple::draw_sensor_belief() const
{
  static const int ySize = 20;
  static const int xSize = 30;
  double yWidth = 0.5*getFieldInfo().yFieldLength/(double)ySize;
  double xWidth = 0.5*getFieldInfo().xFieldLength/(double)xSize;

  FIELD_DRAWING_CONTEXT;
  DebugDrawings::Color white(1.0,1.0,1.0,0.0); // transparent
  DebugDrawings::Color black(0.0,0.0,0.0,1.0);

  double fixedRotation = theSampleSet.getMostLikelySample().rotation;

  // create new sample set
  SampleSet sampleSet(xSize*ySize);
  int idx = 0;

  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));
      sampleSet.samples[idx].translation = point;
      sampleSet.samples[idx].rotation = fixedRotation;
      sampleSet.samples[idx].likelihood = 1.0;
      idx++;
    }//end for
  }//end for

  updateBySensors(sampleSet);

  double maxValue = 0;
  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      maxValue = max(maxValue, sampleSet.samples[idx++].likelihood);
    }//end for
  }//end for

  if(maxValue == 0) return;

  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2<double> point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));

      double t = sampleSet.samples[idx++].likelihood / maxValue;
      DebugDrawings::Color color = black*t + white*(1-t);
      PEN(color, 20);
      FILLBOX(point.x - xWidth, point.y - yWidth, point.x+xWidth, point.y+yWidth);
    }//end for
  }//end for
}//end draw_closest_points<|endoftext|>


<commit_before>namespace mant {
  namespace bbob2009 {
    class GallaghersGaussian101mePeaksFunction : public BlackBoxOptimisationBenchmark2009 {
      public:
        inline explicit GallaghersGaussian101mePeaksFunction(
            const unsigned int numberOfDimensions) noexcept;

        inline virtual void setParameterRotationR(
            const arma::Mat<double>& parameterRotationR);

        inline virtual void setLocalParameterConditioning(
            const arma::Col<double>& localParameterConditioning);

        inline virtual void setLocalParameterTranslation(
            const arma::Mat<double>& localParameterTranslation);

        inline std::string toString() const noexcept override;

      protected:
        const arma::Col<double>::fixed<101> weight_;

        arma::Mat<double> parameterRotationR_;
        arma::Mat<double> localParameterConditioning_;
        arma::Mat<double> localParameterTranslation_;

        inline arma::Col<double> getRandomLocalParameterConditioning() const noexcept;

        inline arma::Mat<double> getRandomLocalParameterTranslation() const noexcept;

        inline double getSoftConstraintsValueImplementation(
            const arma::Col<double>& parameter) const noexcept override;

        inline double getObjectiveValueImplementation(
            const arma::Col<double>& parameter) const noexcept override;

#if defined(MANTELLA_USE_PARALLEL)
        friend class cereal::access;

        template <typename Archive>
        void serialize(
            Archive& archive) noexcept {
          archive(cereal::make_nvp("BlackBoxOptimisationBenchmark2009", cereal::base_class<BlackBoxOptimisationBenchmark2009>(this)));
          archive(cereal::make_nvp("numberOfDimensions", numberOfDimensions_));
          archive(cereal::make_nvp("parameterRotationR", parameterRotationR_));
          archive(cereal::make_nvp("localParameterConditioning", localParameterConditioning_));
          archive(cereal::make_nvp("localParameterTranslation", localParameterTranslation_));
        }

        template <typename Archive>
        static void load_and_construct(
            Archive& archive,
            cereal::construct<GallaghersGaussian101mePeaksFunction>& construct) noexcept {
          unsigned int numberOfDimensions;
          archive(cereal::make_nvp("numberOfDimensions", numberOfDimensions));
          construct(numberOfDimensions);

          archive(cereal::make_nvp("BlackBoxOptimisationBenchmark2009", cereal::base_class<BlackBoxOptimisationBenchmark2009>(construct.ptr())));
          archive(cereal::make_nvp("parameterRotationR", construct->parameterRotationR_));
          archive(cereal::make_nvp("localParameterConditioning", construct->localParameterConditioning_));
          archive(cereal::make_nvp("localParameterTranslation", construct->localParameterTranslation_));
        }
#endif
    };

    //
    // Implementation
    //

    inline GallaghersGaussian101mePeaksFunction::GallaghersGaussian101mePeaksFunction(
        const unsigned int numberOfDimensions) noexcept
      : BlackBoxOptimisationBenchmark2009(numberOfDimensions),
        weight_(arma::join_cols(arma::Col<double>({10}), arma::linspace<arma::Col<double>>(1.1, 9.1, 100))) {
      setParameterRotationR(getRandomRotationMatrix(numberOfDimensions_));
      setLocalParameterConditioning(getRandomLocalParameterConditioning());
      setLocalParameterTranslation(getRandomLocalParameterTranslation());
    }

    inline void GallaghersGaussian101mePeaksFunction::setParameterRotationR(
        const arma::Mat<double>& parameterRotationR) {
      isEqual("The number of rows", parameterRotationR.n_rows, "the number of dimensions", numberOfDimensions_);
      isRotationMatrix("The matrix", parameterRotationR);

      parameterRotationR_ = parameterRotationR;
    }

    inline void GallaghersGaussian101mePeaksFunction::setLocalParameterConditioning(
        const arma::Col<double>& localParameterConditioning) {
      isEqual("The number of rows", localParameterConditioning.n_rows, "the number of dimensions", numberOfDimensions_);
      isEqual("The number of columns", localParameterConditioning.n_elem, "the number of peaks", 101);

      localParameterConditioning_ = localParameterConditioning;
    }

    inline void GallaghersGaussian101mePeaksFunction::setLocalParameterTranslation(
        const arma::Mat<double>& localParameterTranslation) {
      isEqual("The number of rows", localParameterTranslation.n_rows, "the number of dimensions", numberOfDimensions_);
      isEqual("The number of columns", localParameterTranslation.n_cols, "the number of peaks", 101);

      localParameterTranslation_ = localParameterTranslation;
    }

    inline arma::Mat<double> GallaghersGaussian101mePeaksFunction::getRandomLocalParameterConditioning() const noexcept {
      arma::Col<double> conditionings(101);
      conditionings(0) = 49.5;
      conditionings.tail(conditionings.n_elem - 1) = arma::conv_to<arma::Col<double>>::from(getRandomPermutation(conditionings.n_elem - 1));

      arma::Mat<double> localParameterConditioning(numberOfDimensions_, conditionings.n_elem);
      for (std::size_t n = 0; n < conditionings.n_elem; ++n) {
        const double& conditioning = std::pow(10.0, conditionings(n) / 33.0);
        localParameterConditioning.col(n) = getParameterConditioning(conditioning) / std::sqrt(conditioning);
      }

      return localParameterConditioning;
    }

    inline arma::Mat<double> GallaghersGaussian101mePeaksFunction::getRandomLocalParameterTranslation() const noexcept {
      arma::Mat<double> localParameterTranslation = arma::randu<arma::Mat<double>>(numberOfDimensions_, 101) * 10.0 - 5.0;
      localParameterTranslation.col(0) = 0.8 * localParameterTranslation.col(0);

      return localParameterTranslation;
    }

    inline double GallaghersGaussian101mePeaksFunction::getSoftConstraintsValueImplementation(
        const arma::Col<double>& parameter) const noexcept {
      return getBoundConstraintsValue(parameter);
    }

    inline double GallaghersGaussian101mePeaksFunction::getObjectiveValueImplementation(
        const arma::Col<double>& parameter) const noexcept {
      double maximalValue = std::numeric_limits<double>::lowest();
      for (unsigned int k = 0; k < 101; ++k) {
        const arma::Col<double>& locallyTranslatedParameter = parameter - localParameterTranslation_.col(k);
        maximalValue = std::max(maximalValue, weight_(k) * std::exp(-0.5 / static_cast<double>(numberOfDimensions_) * arma::dot(locallyTranslatedParameter, parameterRotationR_.t() * arma::diagmat(localParameterConditioning_.col(k)) * parameterRotationR_ * locallyTranslatedParameter)));
      }

      return std::pow(getOscillatedValue(10.0 - maximalValue), 2.0);
    }

    inline std::string GallaghersGaussian101mePeaksFunction::toString() const noexcept {
      return "bbob2009_gallaghers_gaussian_101me_peaks_function";
    }
  }
}

#if defined(MANTELLA_USE_PARALLEL)
CEREAL_REGISTER_TYPE(mant::bbob2009::GallaghersGaussian101mePeaksFunction);
#endif
<commit_msg>Fixed type<commit_after>namespace mant {
  namespace bbob2009 {
    class GallaghersGaussian101mePeaksFunction : public BlackBoxOptimisationBenchmark2009 {
      public:
        inline explicit GallaghersGaussian101mePeaksFunction(
            const unsigned int numberOfDimensions) noexcept;

        inline virtual void setParameterRotationR(
            const arma::Mat<double>& parameterRotationR);

        inline virtual void setLocalParameterConditioning(
            const arma::Mat<double>& localParameterConditioning);

        inline virtual void setLocalParameterTranslation(
            const arma::Mat<double>& localParameterTranslation);

        inline std::string toString() const noexcept override;

      protected:
        const arma::Col<double>::fixed<101> weight_;

        arma::Mat<double> parameterRotationR_;
        arma::Mat<double> localParameterConditioning_;
        arma::Mat<double> localParameterTranslation_;

        inline arma::Mat<double> getRandomLocalParameterConditioning() const noexcept;

        inline arma::Mat<double> getRandomLocalParameterTranslation() const noexcept;

        inline double getSoftConstraintsValueImplementation(
            const arma::Col<double>& parameter) const noexcept override;

        inline double getObjectiveValueImplementation(
            const arma::Col<double>& parameter) const noexcept override;

#if defined(MANTELLA_USE_PARALLEL)
        friend class cereal::access;

        template <typename Archive>
        void serialize(
            Archive& archive) noexcept {
          archive(cereal::make_nvp("BlackBoxOptimisationBenchmark2009", cereal::base_class<BlackBoxOptimisationBenchmark2009>(this)));
          archive(cereal::make_nvp("numberOfDimensions", numberOfDimensions_));
          archive(cereal::make_nvp("parameterRotationR", parameterRotationR_));
          archive(cereal::make_nvp("localParameterConditioning", localParameterConditioning_));
          archive(cereal::make_nvp("localParameterTranslation", localParameterTranslation_));
        }

        template <typename Archive>
        static void load_and_construct(
            Archive& archive,
            cereal::construct<GallaghersGaussian101mePeaksFunction>& construct) noexcept {
          unsigned int numberOfDimensions;
          archive(cereal::make_nvp("numberOfDimensions", numberOfDimensions));
          construct(numberOfDimensions);

          archive(cereal::make_nvp("BlackBoxOptimisationBenchmark2009", cereal::base_class<BlackBoxOptimisationBenchmark2009>(construct.ptr())));
          archive(cereal::make_nvp("parameterRotationR", construct->parameterRotationR_));
          archive(cereal::make_nvp("localParameterConditioning", construct->localParameterConditioning_));
          archive(cereal::make_nvp("localParameterTranslation", construct->localParameterTranslation_));
        }
#endif
    };

    //
    // Implementation
    //

    inline GallaghersGaussian101mePeaksFunction::GallaghersGaussian101mePeaksFunction(
        const unsigned int numberOfDimensions) noexcept
      : BlackBoxOptimisationBenchmark2009(numberOfDimensions),
        weight_(arma::join_cols(arma::Col<double>({10}), arma::linspace<arma::Col<double>>(1.1, 9.1, 100))) {
      setParameterRotationR(getRandomRotationMatrix(numberOfDimensions_));
      setLocalParameterConditioning(getRandomLocalParameterConditioning());
      setLocalParameterTranslation(getRandomLocalParameterTranslation());
    }

    inline void GallaghersGaussian101mePeaksFunction::setParameterRotationR(
        const arma::Mat<double>& parameterRotationR) {
      isEqual("The number of rows", parameterRotationR.n_rows, "the number of dimensions", numberOfDimensions_);
      isRotationMatrix("The matrix", parameterRotationR);

      parameterRotationR_ = parameterRotationR;
    }

    inline void GallaghersGaussian101mePeaksFunction::setLocalParameterConditioning(
        const arma::Mat<double>& localParameterConditioning) {
      isEqual("The number of rows", localParameterConditioning.n_rows, "the number of dimensions", numberOfDimensions_);
      isEqual("The number of columns", localParameterConditioning.n_elem, "the number of peaks", 101);

      localParameterConditioning_ = localParameterConditioning;
    }

    inline void GallaghersGaussian101mePeaksFunction::setLocalParameterTranslation(
        const arma::Mat<double>& localParameterTranslation) {
      isEqual("The number of rows", localParameterTranslation.n_rows, "the number of dimensions", numberOfDimensions_);
      isEqual("The number of columns", localParameterTranslation.n_cols, "the number of peaks", 101);

      localParameterTranslation_ = localParameterTranslation;
    }

    inline arma::Mat<double> GallaghersGaussian101mePeaksFunction::getRandomLocalParameterConditioning() const noexcept {
      arma::Col<double> conditionings(101);
      conditionings(0) = 49.5;
      conditionings.tail(conditionings.n_elem - 1) = arma::conv_to<arma::Col<double>>::from(getRandomPermutation(conditionings.n_elem - 1));

      arma::Mat<double> localParameterConditioning(numberOfDimensions_, conditionings.n_elem);
      for (std::size_t n = 0; n < conditionings.n_elem; ++n) {
        const double& conditioning = std::pow(10.0, conditionings(n) / 33.0);
        localParameterConditioning.col(n) = getParameterConditioning(conditioning) / std::sqrt(conditioning);
      }

      return localParameterConditioning;
    }

    inline arma::Mat<double> GallaghersGaussian101mePeaksFunction::getRandomLocalParameterTranslation() const noexcept {
      arma::Mat<double> localParameterTranslation = arma::randu<arma::Mat<double>>(numberOfDimensions_, 101) * 10.0 - 5.0;
      localParameterTranslation.col(0) = 0.8 * localParameterTranslation.col(0);

      return localParameterTranslation;
    }

    inline double GallaghersGaussian101mePeaksFunction::getSoftConstraintsValueImplementation(
        const arma::Col<double>& parameter) const noexcept {
      return getBoundConstraintsValue(parameter);
    }

    inline double GallaghersGaussian101mePeaksFunction::getObjectiveValueImplementation(
        const arma::Col<double>& parameter) const noexcept {
      double maximalValue = std::numeric_limits<double>::lowest();
      for (unsigned int k = 0; k < 101; ++k) {
        const arma::Col<double>& locallyTranslatedParameter = parameter - localParameterTranslation_.col(k);
        maximalValue = std::max(maximalValue, weight_(k) * std::exp(-0.5 / static_cast<double>(numberOfDimensions_) * arma::dot(locallyTranslatedParameter, parameterRotationR_.t() * arma::diagmat(localParameterConditioning_.col(k)) * parameterRotationR_ * locallyTranslatedParameter)));
      }

      return std::pow(getOscillatedValue(10.0 - maximalValue), 2.0);
    }

    inline std::string GallaghersGaussian101mePeaksFunction::toString() const noexcept {
      return "bbob2009_gallaghers_gaussian_101me_peaks_function";
    }
  }
}

#if defined(MANTELLA_USE_PARALLEL)
CEREAL_REGISTER_TYPE(mant::bbob2009::GallaghersGaussian101mePeaksFunction);
#endif
<|endoftext|>