<commit_before>// Test function outside namespace...
int func_outside_namespace();

// Test class outside namespace...
class class_outside_namespace
{
	// Test basic types...
	char a;
	wchar_t b;
	short c;
	int d;
	long e;
	long long f;
	unsigned g;
	unsigned short h;
	unsigned long long i;
};

// Test variables outside namespace...
class_outside_namespace cls1;
int variable;

// Test a namespace...
namespace mynamespace
{
// Test class with a reserved Java name...
class String
{
	int m_count;

	// Test constructor initializer lists...
	String(int i) : m_count(i) { }
}

// Test basic template class...
template <class T>
class mypair {
	// Test array of template parameter types...
	T values [2];
	// Test access modifier...
	public:
	// Test constructor using template parameter types...
	mypair (T first, T second)
	{
		// Test array access...
		values[0] = first;
		values[1] = second;
	}
};

// Test template class with two parameters...
template <class T, class V>
class HashMap
{
	T one;
	V two;
};

template<class V, class T> class CT
{
	// Test using template types in as template parameters...
	HashMap<V, int> three;
	T one;
	V two;

};

// Test anonymous enums...
enum { value1, value2 };
enum { value3, value4 };

class foo
{
	foo() { }
	foo(int i) {
		// Test use of anonymous enums values
		// and assigning enum values to int...
		i = value1;
		i = value4;
	}
	foo(int i, short t) {}

	// Test a function declaration with default values...
	int func_with_defaults(int one, int two = 5);

	// Test a function definition with default values...
	int func_with_defaults_and_definition(int one, int two = 3)
	{
		two = 4;
		// Test a return statement...
		return two;
	}
};

// Test a class with a parent class...
class test : public foo {

	// Test an enclosed enumeration...
	enum test_enum
	{
		val1,
		// Test giving enumerators values...
		val2 = 1020,
		val3,
		val4 = 1045
	};

	// Test an enclosed class...
	class subby
	{
		int k;
	};

	// Test value in class with implicit constructor...
	foo test_foo_with_init;

	// Test value in class with initializer value...
	foo test_foo_with_int_constructor(55);

	// Test bit fields...
	int test_with_bit_field : 5;

	int func2()
	{
		// Test setting of bit field...
		test_with_bit_field = 8;
		// Test getting of bit field...
		return test_with_bit_field;
	}

	// Test function for pointers and references...
	int func3(foo a, foo * b, foo ** c, foo& d, const foo& e, int& f)
	{
		// Test declaration in while...
		while (foo* j = b) { }

		// Test declaration in if...
		if (foo * l = b) { }

		// Test declaration in for...
		for (foo * a5 = b;a5;a5 = b) { }

		// Test while...
		while (b) { }

		// Test if...
		if (b) { }

		// Test standard looking for...
		for (int i = 0; i < 10; i++)
		{
			// Test nested for...
			for (i = 1; i < 5; i++)
			{
				// Test break statement...
				break;
			}
			// Test continue statement...
			continue;
		}

		// Test qualified enum access...
		int i = test_enum::val1 + test_enum::val2;

		// Test various pointer to basic types...
		char * ptr1;
		int ** ptr2;
		short * ptr3;
		unsigned * ptr4;
		bool * ptr5;
		// Test multiple declarators on with one declaration...
		int * ptr7, * ptr6;

		// Test address of operator...
		ptr7 = &a;

		// Test basic pointer assignment...
		ptr7 = ptr6;

		// Test number literals...
		return 1072;
	}

	foo func4(int a, int b, short c = 55)
	{
		// Test implicity constructors with array...
		foo sd[15];

		// Test pointers to composite type...
		foo * foo1;
		foo ** foo2;

		// Test references to composite type...
		foo& foo3 = sd[1];

		// Test constructor call...
		foo foo4(1, 2);

		// Test implicity copy constructor call...
		foo foo5 = foo4;

		// Test pointer assignment from this...
		foo * foo6 = this;

		// Test copy from this...
		foo foo7 = *this;

		// Test bracketed expression...
		int d = (1 + 2) * 4;

		// Test && with non boolean expression...
		bool mybool1 = foo1 && foo6 && true;

		// Test || with non boolean expression...
		bool mybool2 = foo1 || foo6 || false;

		// Test function call...
		func2();

		// Test +=...
		d += 10;

		// Test switch...
		switch (d)
		{
			// Test fall through...
			case 1:
			case 2:
				break;
			default:
				return foo7;
		}

		// Test -> operator...
		foo6->func_with_defaults_and_definition(100);

		// Test dereference and call...
		(*foo6).func_with_defaults_and_definition(100, 2);

		// Test copy constructor on return...
		return sd[2];
	}
};
};

<commit_msg>Added test cases for struct(passed), union(passed) and anonymous class(failed).<commit_after>// Test function outside namespace...
int func_outside_namespace();

// Test class outside namespace...
class class_outside_namespace
{
	// Test basic types...
	char a;
	wchar_t b;
	short c;
	int d;
	long e;
	long long f;
	unsigned g;
	unsigned short h;
	unsigned long long i;
};

// Test variables outside namespace...
class_outside_namespace cls1;
int variable;

// Test a namespace...
namespace mynamespace
{
// Test class with a reserved Java name...
class String
{
	int m_count;

	// Test constructor initializer lists...
	String(int i) : m_count(i) { }
}

// Test basic template class...
template <class T>
class mypair {
	// Test array of template parameter types...
	T values [2];
	// Test access modifier...
	public:
	// Test constructor using template parameter types...
	mypair (T first, T second)
	{
		// Test array access...
		values[0] = first;
		values[1] = second;
	}
};

// Test template class with two parameters...
template <class T, class V>
class HashMap
{
	T one;
	V two;
};

template<class V, class T> class CT
{
	// Test using template types in as template parameters...
	HashMap<V, int> three;
	T one;
	V two;

};

// Test anonymous enums...
enum { value1, value2 };
enum { value3, value4 };

class foo
{
	foo() { }
	foo(int i) {
		// Test use of anonymous enums values
		// and assigning enum values to int...
		i = value1;
		i = value4;
	}
	foo(int i, short t) {}

	// Test a function declaration with default values...
	int func_with_defaults(int one, int two = 5);

	// Test a function definition with default values...
	int func_with_defaults_and_definition(int one, int two = 3)
	{
		two = 4;
		// Test a return statement...
		return two;
	}
};

// Test a class with a parent class...
class test : public foo {

	// Test an enclosed enumeration...
	enum test_enum
	{
		val1,
		// Test giving enumerators values...
		val2 = 1020,
		val3,
		val4 = 1045
	};

	// Test an anonymous enclosed class...
	class { int a, b; } anon_class1, anon_class2;

	// Test an enclosed class...
	class subby
	{
		int k;
	};

	// Test value in class with implicit constructor...
	foo test_foo_with_init;

	// Test value in class with initializer value...
	foo test_foo_with_int_constructor(55);

	// Test bit fields...
	int test_with_bit_field : 5;

	int func2()
	{
		// Test setting of bit field...
		test_with_bit_field = 8;
		// Test getting of bit field...
		return test_with_bit_field;
	}

	// Test function for pointers and references...
	int func3(foo a, foo * b, foo ** c, foo& d, const foo& e, int& f)
	{
		// Test declaration in while...
		while (foo* j = b) { }

		// Test declaration in if...
		if (foo * l = b) { }

		// Test declaration in for...
		for (foo * a5 = b;a5;a5 = b) { }

		// Test while...
		while (b) { }

		// Test if...
		if (b) { }

		// Test standard looking for...
		for (int i = 0; i < 10; i++)
		{
			// Test nested for...
			for (i = 1; i < 5; i++)
			{
				// Test break statement...
				break;
			}
			// Test continue statement...
			continue;
		}

		// Test qualified enum access...
		int i = test_enum::val1 + test_enum::val2;

		// Test various pointer to basic types...
		char * ptr1;
		int ** ptr2;
		short * ptr3;
		unsigned * ptr4;
		bool * ptr5;
		// Test multiple declarators on with one declaration...
		int * ptr7, * ptr6;

		// Test address of operator...
		ptr7 = &a;

		// Test basic pointer assignment...
		ptr7 = ptr6;

		// Test number literals...
		return 1072;
	}

	foo func4(int a, int b, short c = 55)
	{
		// Test implicity constructors with array...
		foo sd[15];

		// Test pointers to composite type...
		foo * foo1;
		foo ** foo2;

		// Test references to composite type...
		foo& foo3 = sd[1];

		// Test constructor call...
		foo foo4(1, 2);

		// Test implicity copy constructor call...
		foo foo5 = foo4;

		// Test pointer assignment from this...
		foo * foo6 = this;

		// Test copy from this...
		foo foo7 = *this;

		// Test bracketed expression...
		int d = (1 + 2) * 4;

		// Test && with non boolean expression...
		bool mybool1 = foo1 && foo6 && true;

		// Test || with non boolean expression...
		bool mybool2 = foo1 || foo6 || false;

		// Test function call...
		func2();

		// Test +=...
		d += 10;

		// Test switch...
		switch (d)
		{
			// Test fall through...
			case 1:
			case 2:
				break;
			default:
				return foo7;
		}

		// Test -> operator...
		foo6->func_with_defaults_and_definition(100);

		// Test dereference and call...
		(*foo6).func_with_defaults_and_definition(100, 2);

		// Test copy constructor on return...
		return sd[2];
	}

	// Test a union...
	union test_union
	{
		int a;
		float b;
	};

	// Test a struct...
	struct test_struct
	{
		int a;
		int b;
	};
};

// Test a namepace level union...
union test_union2
{
	short a, b;
};

};

<|endoftext|>


<commit_before>#include "Polynomial.h"

#define BOOST_TEST_MODULE Polynomial
#include <boost/test/included/unit_test.hpp>

using namespace std;

// 0 degree polynomial
BOOST_AUTO_TEST_CASE(zero_degree)
{
  Polynomial<double> p0(0);
  p0[0] = 42.0;

  BOOST_CHECK(p0.degree() == 0);
  BOOST_CHECK(p0(0.123) == 42.0);
}

// first degree polynomial
BOOST_AUTO_TEST_CASE(first_degree)
{
  Polynomial<double> p1(1);
  p1[0] = -1.0;
  p1[1] = 1.0;

  BOOST_CHECK(p1.degree() == 1);
  BOOST_CHECK(p1(0.0) == -1.0);
  BOOST_CHECK(p1(1.0) == 0.0);
  BOOST_CHECK(p1(2.0) == 1.0);
  BOOST_CHECK(p1(-1.0) == -2.0);
}

// second degree polynomial
BOOST_AUTO_TEST_CASE(second_degree)
{
  double c[3] ;

  c[0] = 2.0 ;
  c[1] = -1.0;
  c[2] = 3.0 ;

  Polynomial<double> p2(c, 2) ;

  BOOST_CHECK(p2.degree() == 2);
  BOOST_CHECK(p2(0.0) == 2.0);
  BOOST_CHECK(p2(1.0) == 4.0);
  BOOST_CHECK(p2(2.0) == 12.0);
}

// third degree polynomial
BOOST_AUTO_TEST_CASE(third_degree)
{
  vector<float> c;

  c.push_back(-1.0);
  c.push_back(1.0);
  c.push_back(-2.0);
  c.push_back(3.0);

  Polynomial<float> p3(c);

  BOOST_CHECK(p3.degree() == 3);
  BOOST_CHECK(p3(0.0) == -1.0);
  BOOST_CHECK(p3(1.0) == 1.0);
  BOOST_CHECK(p3(-1.0) == -7.0);
}

// increment degree
BOOST_AUTO_TEST_CASE(increment_degree)
{
  double c[3] ;

  c[0] = 2.0 ;
  c[1] = -1.0;
  c[2] = 3.0 ;

  Polynomial<double> p(c, 2) ;
  BOOST_CHECK(p.degree() == 2);

  p.IncrementDegree(12.0);
  BOOST_CHECK(p.degree() == 3);
  BOOST_CHECK(p[0] == 2);
  BOOST_CHECK(p[1] == -1.0);
  BOOST_CHECK(p[2] == 3.0);
  BOOST_CHECK(p[3] == 12.0);
}
<commit_msg>added an addtional test<commit_after>#include "Polynomial.h"

#include <complex>

#define BOOST_TEST_MODULE Polynomial
#include <boost/test/included/unit_test.hpp>

using namespace std;

// 0 degree polynomial
BOOST_AUTO_TEST_CASE(zero_degree)
{
  Polynomial<double> p0(0);
  p0[0] = 42.0;

  BOOST_CHECK(p0.degree() == 0);
  BOOST_CHECK(p0(0.123) == 42.0);
}

// first degree polynomial
BOOST_AUTO_TEST_CASE(first_degree)
{
  Polynomial<double> p1(1);
  p1[0] = -1.0;
  p1[1] = 1.0;

  BOOST_CHECK(p1.degree() == 1);
  BOOST_CHECK(p1(0.0) == -1.0);
  BOOST_CHECK(p1(1.0) == 0.0);
  BOOST_CHECK(p1(2.0) == 1.0);
  BOOST_CHECK(p1(-1.0) == -2.0);
}

// second degree polynomial
BOOST_AUTO_TEST_CASE(second_degree)
{
  double c[3] ;

  c[0] = 2.0 ;
  c[1] = -1.0;
  c[2] = 3.0 ;

  Polynomial<double> p2(c, 2) ;

  BOOST_CHECK(p2.degree() == 2);
  BOOST_CHECK(p2(0.0) == 2.0);
  BOOST_CHECK(p2(1.0) == 4.0);
  BOOST_CHECK(p2(2.0) == 12.0);
}

// third degree polynomial
BOOST_AUTO_TEST_CASE(third_degree)
{
  vector<float> c;

  c.push_back(-1.0);
  c.push_back(1.0);
  c.push_back(-2.0);
  c.push_back(3.0);

  Polynomial<float> p3(c);

  BOOST_CHECK(p3.degree() == 3);
  BOOST_CHECK(p3(0.0) == -1.0);
  BOOST_CHECK(p3(1.0) == 1.0);
  BOOST_CHECK(p3(-1.0) == -7.0);
}

// increment degree
BOOST_AUTO_TEST_CASE(increment_degree)
{
  double c[3] ;

  c[0] = 2.0 ;
  c[1] = -1.0;
  c[2] = 3.0 ;

  Polynomial<double> p(c, 2) ;
  BOOST_CHECK(p.degree() == 2);

  p.IncrementDegree(12.0);
  BOOST_CHECK(p.degree() == 3);
  BOOST_CHECK(p[0] == 2);
  BOOST_CHECK(p[1] == -1.0);
  BOOST_CHECK(p[2] == 3.0);
  BOOST_CHECK(p[3] == 12.0);
}

// increment degree
BOOST_AUTO_TEST_CASE(complex_poly)
{
  Polynomial<complex<double> > p(2);
  p[0] = 1.0;
  p[1] = 0.0;
  p[2] = 1.0;

  BOOST_CHECK(p.degree() == 2);
  BOOST_CHECK(p(1.0i) == 0.0);

  p.IncrementDegree();
  BOOST_CHECK(p.degree() == 3);
  BOOST_CHECK(p[3] == 0.0);

  p[3] = 2.0 - 3.0i;

  complex<double> result = p(2.0 + 1.0i);

  BOOST_CHECK(result.real() == 41);
  BOOST_CHECK(result.imag() == 20);
}
<|endoftext|>


<commit_before>
#include <exception>
#include <iostream>

#include <cassert>

#define _DBC_STRINGIZE(x) #x
#define _DBC_STR(x) _DBC_STRINGIZE(x)

class DbcException : public std::exception {

public:
  DbcException(const char* message) : m_message(message) {

  }

    virtual const char* what() const throw() {return m_message;}

     virtual ~DbcException() throw() {}
private:

  const char* m_message;
};

void _dbc_general_assert_no_inv(bool cond, const char * msg) {if(!cond) {throw  DbcException(msg);}}

#define DBC_PRECOND_NO_INV(cond) _dbc_general_assert_no_inv(cond, "Precondition failed " #cond " in " __FILE__ " (" _DBC_STR(__LINE__)")");

#define DBC_INV() _dbc_general_assert_no_inv(invariant(), "Invariant check failed in " __FILE__ " (" _DBC_STR(__LINE__)")");

class MyClass {

public:
  void precondFailingMethod() {
    DBC_PRECOND_NO_INV(m_a == 1);
  }

  void setA(int a) {m_a=a;}

  int getA() const {return m_a;}

private:
  int m_a;
};

void testPreconditionFailing() {
  bool exceptionCaught = false;
  MyClass c;
  c.setA(0);
  try {
    c.precondFailingMethod();
  }
  catch(DbcException &e) {
    std::cout << e.what();
    exceptionCaught = true;
  }
  assert(exceptionCaught);
}

int main(int argn, char** argv) {

    testPreconditionFailing();

    return 0;
}

<commit_msg>Precondition test implemented with gtest<commit_after>

#include <exception>
#include <stdexcept>

#include <iostream>

#include <cassert>

#include "gtest/gtest.h"

#define _DBC_STRINGIZE(x) #x
#define _DBC_STR(x) _DBC_STRINGIZE(x)

class DbcException : public std::exception {

public:
  DbcException(const char* message) : m_message(message) {

  }

    virtual const char* what() const throw() {return m_message;}

     virtual ~DbcException() throw() {}
private:

  const char* m_message;
};

void _dbc_general_assert_no_inv(bool cond, const char * msg) {if(!cond) {throw  DbcException(msg);}}

#define DBC_PRECOND_NO_INV(cond) _dbc_general_assert_no_inv(cond, "Precondition failed " #cond " in " __FILE__ " (" _DBC_STR(__LINE__)")");

#define DBC_INV() _dbc_general_assert_no_inv(invariant(), "Invariant check failed in " __FILE__ " (" _DBC_STR(__LINE__)")");

class MyClass {

public:
  void precondFailingMethod() {
    DBC_PRECOND_NO_INV(m_a == 1);
  }

  void setA(int a) {m_a=a;}

  int getA() const {return m_a;}

private:
  int m_a;
};

TEST(PreconditionTest, precondFail)
{
  bool exceptionCaught = false;
  MyClass c;
  c.setA(0);
  ASSERT_THROW(c.precondFailingMethod(), DbcException);
 }

<|endoftext|>


<commit_before>/*
 * nnet test
 * 2016.06.06
 * author:soma62jp
 *                  */

#include "test.h"

using namespace std;

nnet::nnet(int inputnum,int hiddennum,int outputnum,int patternnum):
      inputnum(inputnum)
     ,hiddennum(hiddennum)
     ,outputnum(outputnum)
     ,patternnum(patternnum)
     ,Eta(0.75)
     ,Alpha(0.8)
     ,ErrorEv(0.08)
     ,Rlow(-0.30)
     ,Rhigh(0.30)
{
  //this->inputnum=inputnum;
  //this->hiddennum=hiddennum;
  //this->outputnum=outputnum;

  X_h = new double[hiddennum];
  X_o = new double[outputnum];

  bias_h = new double[hiddennum];
  bias_o = new double[outputnum];
  bias_h_prev = new double[hiddennum];
  bias_o_prev = new double[outputnum];

  // pattern*input
  X_i = new double*[patternnum];
  for(int i=0;i<patternnum;i++){
    X_i[i] = new double[inputnum];
  }

  // pattern*output
  T_signal = new double*[patternnum];
  for(int i=0;i<patternnum;i++){
    T_signal[i] = new double[outputnum];
  }

  // hidden*input
  W_itoh = new double*[hiddennum];
  for(int i=0;i<hiddennum;i++){
    W_itoh[i] = new double[inputnum];
  }
  W_itoh_prev = new double*[hiddennum];
  for(int i=0;i<hiddennum;i++){
    W_itoh_prev[i] = new double[inputnum];
  }

  // output*hidden
  W_htoo = new double*[outputnum];
  for(int i=0;i<outputnum;i++){
    W_htoo[i] = new double[hiddennum];
  }
  W_htoo_prev = new double*[outputnum];
  for(int i=0;i<outputnum;i++){
    W_htoo_prev[i] = new double[hiddennum];
  }

  //initialize parameter
  srand((unsigned int)time(NULL));

  for(int i=0;i<hiddennum;i++){
    for(int j=0;j<inputnum;j++){
      W_itoh[i][j]=urand();
    }
  }

  for(int i=0;i<outputnum;i++){
    for(int j=0;j<hiddennum;j++){
      W_htoo[i][j]=urand();
    }
  }

}

nnet::~nnet()
{

  delete [] X_h;
  delete [] X_o;

  delete [] bias_h;
  delete [] bias_o;
  delete [] bias_h_prev;
  delete [] bias_o_prev;

  for( int i=0; i<patternnum; i++ ) {
    delete[] X_i[i];
  }
  delete [] X_i;

  for( int i=0; i<patternnum; i++ ) {
    delete[] T_signal[i];
  }
  delete [] T_signal;

  for( int i=0; i<hiddennum; i++ ) {
    delete[] W_itoh[i];
  }
  delete [] W_itoh;

  for( int i=0; i<hiddennum; i++ ) {
    delete[] W_itoh_prev[i];
  }
  delete [] W_itoh_prev;

  for( int i=0; i<outputnum; i++ ) {
    delete [] W_htoo[i];
  }
  delete [] W_htoo;

  for( int i=0; i<outputnum; i++ ) {
    delete [] W_htoo_prev[i];
  }
  delete [] W_htoo_prev;

}

void nnet::foward_propagation(const int pnum)
{
  int i,j;
  double sum;

  // 入力層ー＞隠れ層
  for(i=0;i<hiddennum;i++){
    sum=0;
    for(j=0;j<inputnum;j++){
      // 重み×入力値
      sum+=W_itoh[i][j]*X_i[pnum][j];
    }
    // 重み×入力値の総和にバイアス項を足してアクティベーション関数に通したものが中間層入力
    X_h[i] = activationFunc(sum+bias_h[i]);
  }

  // 隠れ層ー＞出力層
  for(i=0;i<outputnum;i++){
    sum=0;
    for(j=0;j<hiddennum;j++){
      // 重み×中間層入力
      sum+=W_htoo[i][j]*X_h[j];
    }
    // 重み×中間層入力値の総和にバイアス項を足してアクティベーション関数に通したものが出力層
    X_o[i]=activationFunc(sum+bias_o[i]);
  }

}

void nnet::back_propagation(const int pnum)
{
  int i,j;
  double sum;
  double *dwih = new double[hiddennum];   // 隠れ層での学習信号
  double *dwho = new double[outputnum];   // 出力層での学習信号

  // 出力層から計算
  for(i=0;i<outputnum;i++){
    // 出力層での学習信号=(教師信号-出力）*出力*(1-出力)
    // 出力*(1-出力)はシグモイド関数の微分
    dwho[i]=(T_signal[pnum][i]-X_o[i]) * X_o[i] * (1.0-X_o[i]);
  }

  for(i=0;i<hiddennum;i++){
    sum=0;
    for(j=0;j<outputnum;j++){
      // 重みの変化量
      W_htoo_prev[j][i]=Eta*dwho[j]*X_h[i]+Alpha*W_htoo_prev[j][i];
      W_htoo[j][i]+=W_htoo_prev[j][i];
      sum = dwho[j]*W_htoo[j][i];
    }
    dwih[i]=X_h[i]*(1-X_h[i])*sum;
  }

  for(i=0;i<outputnum;i++){
    bias_o_prev[i] = Eta*dwho[i]+Alpha*bias_o_prev[i];
    bias_o[i]+=bias_o_prev[i];
  }

  for(i=0;i<inputnum;i++){
    for(j=0;j<hiddennum;j++){
      W_itoh_prev[j][i]=Eta*dwih[j]*X_i[pnum][i]+Alpha*W_itoh_prev[j][i];
      W_itoh[j][i]+=W_itoh_prev[j][i];
    }
  }

  for(i=0;i<hiddennum;i++){
    bias_h_prev[i]=Eta*dwih[i]+Alpha+bias_h_prev[i];
    bias_h[i]+=bias_h_prev[i];
  }

  delete dwih;
  delete dwho;

}

int main()
{
  cout << "Hellow World!" << endl;
}
<commit_msg>commit comment<commit_after>/*
 * nnet test
 * 2016.06.06
 * author:soma62jp
 *                  */

#include "test.h"

using namespace std;

nnet::nnet(int inputnum,int hiddennum,int outputnum,int patternnum):
      inputnum(inputnum)
     ,hiddennum(hiddennum)
     ,outputnum(outputnum)
     ,patternnum(patternnum)
     ,Eta(0.75)
     ,Alpha(0.8)
     ,ErrorEv(0.08)
     ,Rlow(-0.30)
     ,Rhigh(0.30)
{
  //this->inputnum=inputnum;
  //this->hiddennum=hiddennum;
  //this->outputnum=outputnum;

  X_h = new double[hiddennum];
  X_o = new double[outputnum];

  bias_h = new double[hiddennum];
  bias_o = new double[outputnum];
  bias_h_prev = new double[hiddennum];
  bias_o_prev = new double[outputnum];

  // pattern*input
  X_i = new double*[patternnum];
  for(int i=0;i<patternnum;i++){
    X_i[i] = new double[inputnum];
  }

  // pattern*output
  T_signal = new double*[patternnum];
  for(int i=0;i<patternnum;i++){
    T_signal[i] = new double[outputnum];
  }

  // hidden*input
  W_itoh = new double*[hiddennum];
  for(int i=0;i<hiddennum;i++){
    W_itoh[i] = new double[inputnum];
  }
  W_itoh_prev = new double*[hiddennum];
  for(int i=0;i<hiddennum;i++){
    W_itoh_prev[i] = new double[inputnum];
  }

  // output*hidden
  W_htoo = new double*[outputnum];
  for(int i=0;i<outputnum;i++){
    W_htoo[i] = new double[hiddennum];
  }
  W_htoo_prev = new double*[outputnum];
  for(int i=0;i<outputnum;i++){
    W_htoo_prev[i] = new double[hiddennum];
  }

  //initialize parameter
  srand((unsigned int)time(NULL));

  for(int i=0;i<hiddennum;i++){
    for(int j=0;j<inputnum;j++){
      W_itoh[i][j]=urand();
    }
  }

  for(int i=0;i<outputnum;i++){
    for(int j=0;j<hiddennum;j++){
      W_htoo[i][j]=urand();
    }
  }

}

nnet::~nnet()
{

  delete [] X_h;
  delete [] X_o;

  delete [] bias_h;
  delete [] bias_o;
  delete [] bias_h_prev;
  delete [] bias_o_prev;

  for( int i=0; i<patternnum; i++ ) {
    delete[] X_i[i];
  }
  delete [] X_i;

  for( int i=0; i<patternnum; i++ ) {
    delete[] T_signal[i];
  }
  delete [] T_signal;

  for( int i=0; i<hiddennum; i++ ) {
    delete[] W_itoh[i];
  }
  delete [] W_itoh;

  for( int i=0; i<hiddennum; i++ ) {
    delete[] W_itoh_prev[i];
  }
  delete [] W_itoh_prev;

  for( int i=0; i<outputnum; i++ ) {
    delete [] W_htoo[i];
  }
  delete [] W_htoo;

  for( int i=0; i<outputnum; i++ ) {
    delete [] W_htoo_prev[i];
  }
  delete [] W_htoo_prev;

}

void nnet::foward_propagation(const int pnum)
{
  int i,j;
  double sum;

  // 入力層ー＞隠れ層
  for(i=0;i<hiddennum;i++){
    sum=0;
    for(j=0;j<inputnum;j++){
      // 重み×入力値
      sum+=W_itoh[i][j]*X_i[pnum][j];
    }
    // 重み×入力値の総和にバイアス項を足してアクティベーション関数に通したものが中間層入力
    X_h[i] = activationFunc(sum+bias_h[i]);
  }

  // 隠れ層ー＞出力層
  for(i=0;i<outputnum;i++){
    sum=0;
    for(j=0;j<hiddennum;j++){
      // 重み×中間層入力
      sum+=W_htoo[i][j]*X_h[j];
    }
    // 重み×中間層入力値の総和にバイアス項を足してアクティベーション関数に通したものが出力層
    X_o[i]=activationFunc(sum+bias_o[i]);
  }

}

void nnet::back_propagation(const int pnum)
{
  int i,j;
  double sum;
  double *dwih = new double[hiddennum];   // 隠れ層での学習信号
  double *dwho = new double[outputnum];   // 出力層での学習信号

  // 出力層から計算
  for(i=0;i<outputnum;i++){
    // 出力層での学習信号=(教師信号-出力）*出力*(1-出力)
    // 出力*(1-出力)はシグモイド関数の微分
    dwho[i]=(T_signal[pnum][i]-X_o[i]) * X_o[i] * (1.0-X_o[i]);
  }

  for(i=0;i<hiddennum;i++){
    sum=0;
    for(j=0;j<outputnum;j++){
      // 重みの変化量
      W_htoo_prev[j][i]=Eta*dwho[j]*X_h[i]+Alpha*W_htoo_prev[j][i];
      W_htoo[j][i]+=W_htoo_prev[j][i];
      sum = dwho[j]*W_htoo[j][i];
    }
    dwih[i]=X_h[i]*(1-X_h[i])*sum;
  }

  for(i=0;i<outputnum;i++){
    bias_o_prev[i] = Eta*dwho[i]+Alpha*bias_o_prev[i];
    bias_o[i]+=bias_o_prev[i];
  }

  for(i=0;i<inputnum;i++){
    for(j=0;j<hiddennum;j++){
      W_itoh_prev[j][i]=Eta*dwih[j]*X_i[pnum][i]+Alpha*W_itoh_prev[j][i];
      W_itoh[j][i]+=W_itoh_prev[j][i];
    }
  }

  for(i=0;i<hiddennum;i++){
    bias_h_prev[i]=Eta*dwih[i]+Alpha+bias_h_prev[i];
    bias_h[i]+=bias_h_prev[i];
  }

  delete dwih;
  delete dwho;

}

int main()
{

  cout << "Hellow World!" << endl;
}
<|endoftext|>


<commit_before>//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#define ZMQ_PUB_PORT "tcp://*:7580"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include "pixy.h"

#include "zmq.h"

#define BLOCK_BUFFER_SIZE    25

// Pixy Block buffer //
struct Block blocks[BLOCK_BUFFER_SIZE];

static bool run_flag = true;

void handle_SIGINT(int unused)
{
  // On CTRL+C - abort! //

  run_flag = false;
}

int str_to_msg(char* send, zmq_msg_t* msg)
{
    int err;
    int len = strlen(send);
    err = zmq_msg_init_size(msg, len);
    if (err != 0){
        printf("ERROR: zmq_msg_init_size failed with %s\n", zmq_strerror(zmq_errno()));
        return err;
    }
    memcpy(zmq_msg_data(msg), send, len);
    return 0;
}

int send_int_as_hex(uint16_t send, void* publisher, bool more)
{
    char buf[8];
    int err;
    zmq_msg_t msg;

    err = sprintf(buf, "0x%04x", send);
    if (err < 0)
    {
        printf("ERROR: Failed to format message\n");
        return -1;
    }
    err = str_to_msg(buf, &msg);
    if (err != 0)
    {
        zmq_msg_close(&msg);
        return err;
    }
    if (more)
    {
        err = zmq_send(publisher, &msg, ZMQ_SNDMORE);
    }
    else
    {
        err = zmq_send(publisher, &msg, 0);
    }
    zmq_msg_close(&msg);
    if (err != 0)
    {
        printf("ERROR: zmq_send failed with %s\n", zmq_strerror(zmq_errno()));
        return err;
    }
    return 0;
}

int main(int argc, char * argv[])
{
	// ZMQ variables
	int err;
	void* context = zmq_init(1);
	void* publisher = zmq_socket(context, ZMQ_PUB);

	// Prepare ZMQ context and publisher
	err = zmq_bind(publisher, ZMQ_PUB_PORT);
	if(err != 0){
		printf("zmq_bind failed with %s\n", zmq_strerror(zmq_errno()));
		zmq_close(publisher);
		zmq_term(context);
		exit(1);
	}

    zmq_msg_t msgpart;

// Pixy example below
  int      i = 0;
  int      index;
  int      blocks_copied;
  int      pixy_init_status;
  char     buf[128];

  // Catch CTRL+C (SIGINT) signals //
  signal(SIGINT, handle_SIGINT);

  printf("Hello Pixy:\n libpixyusb Version: %s\n", __LIBPIXY_VERSION__);

  // Connect to Pixy //
  pixy_init_status = pixy_init();

  // Was there an error initializing pixy? //
  if(!pixy_init_status == 0)
  {
    // Error initializing Pixy //
    printf("pixy_init(): ");
    pixy_error(pixy_init_status);

    return pixy_init_status;
  }

  // Request Pixy firmware version //
  {
    uint16_t major;
    uint16_t minor;
    uint16_t build;
    int      return_value;

    return_value = pixy_get_firmware_version(&major, &minor, &build);

    if (return_value) {
      // Error //
      printf("Failed to retrieve Pixy firmware version. ");
      pixy_error(return_value);
		zmq_close(publisher);
		zmq_term(context);

      return return_value;
    } else {
      // Success //
      printf(" Pixy Firmware Version: %d.%d.%d\n", major, minor, build);
    }
  }

#if 0
  // Pixy Command Examples //
  {
    int32_t response;
    int     return_value;

    // Execute remote procedure call "cam_setAWB" with one output (host->pixy) parameter (Value = 1)
    //
    //   Parameters:                 Notes:
    //
    //   pixy_command("cam_setAWB",  String identifier for remote procedure
    //                        0x01,  Length (in bytes) of first output parameter
    //                           1,  Value of first output parameter
    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
    //                   &response,  Pointer to memory address for return value from remote procedure call
    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
    //

    // Enable auto white balance //
    pixy_command("cam_setAWB", UINT8(0x01), END_OUT_ARGS,  &response, END_IN_ARGS);

    // Execute remote procedure call "cam_getAWB" with no output (host->pixy) parameters
    //
    //   Parameters:                 Notes:
    //
    //   pixy_command("cam_setAWB",  String identifier for remote procedure
    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
    //                   &response,  Pointer to memory address for return value from remote procedure call
    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
    //

    // Get auto white balance //
    return_value = pixy_command("cam_getAWB", END_OUT_ARGS, &response, END_IN_ARGS);

    // Set auto white balance back to disabled //
    pixy_command("cam_setAWB", UINT8(0x00), END_OUT_ARGS,  &response, END_IN_ARGS);
  }
#endif

    // Frame grab variables
    unsigned char current_frame[72000]; // ~largest possible given current hardware
    unsigned char *pixels;  //returned pointer to video frame buffer
    int32_t response, fourcc;
    int8_t renderflags;
    int return_value, res;
    uint16_t width, height;
    uint32_t  numPixels;

  printf("Detecting blocks...\n");
  while(run_flag)
  {

/**
 * Disable for now
    // Stop block detection for grabbing frame
    return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);

    response = 0;

    return_value = pixy_command("cam_getFrame",  // String id for remote procedure
                             0x01, 0x21,      // mode 0 = 1280x800 25 fps
                             0x02,   0,        // xoffset
                             0x02,   0,         // yoffset
                             0x02, 320,       // width
                             0x02, 200,       // height
                             0,              // separator
                             &response,      // pointer to mem address for return value
                            &fourcc,         //contrary to docs, the next 5 args are needed
                            &renderflags,
                            &width,
                            &height,
                            &numPixels,
                             &pixels,        // pointer to mem address for returned frame
                             0);
    printf("getFrame returned %d response %d\n", return_value, response);
    printf("returned w %d h %d npix %d\n",width,height,numPixels);

    if (return_value == 0)
    {
        memcpy(&current_frame, pixels,numPixels);
        // TODO push frame over gst
    }

    // Restart block detection
    return_value = pixy_command("run", END_OUT_ARGS, &response, END_IN_ARGS);
    if (return_value != 0)
    {
        printf("ERROR: Restarting block detection failed: return_value %d response %d\n", return_value, response);
        run_flag = false;
        continue;
    }
*/

    // Wait for new blocks to be available //
    if (!pixy_blocks_are_new())
    {
        // Yield
        //sleep(0);
        continue;
    }

    // Get blocks from Pixy //
    blocks_copied = pixy_get_blocks(BLOCK_BUFFER_SIZE, &blocks[0]);

    if(blocks_copied < 0) {
      // Error: pixy_get_blocks //
      printf("pixy_get_blocks(): ");
      pixy_error(blocks_copied);
      continue;
    }

    // Display received blocks //
    for(index = 0; index != blocks_copied; ++index)
    {
        switch (blocks[index].type)
        {
            case 0: // Normal
            {
                /*
                blocks[index].print(buf);
                printf("Sending NORM: %s\n", buf);
                */

                // Topic
                err = str_to_msg("NORM", &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                err = send_int_as_hex(blocks[index].signature, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].x, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].y, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].width, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].height, publisher, false);
                if (err != 0)
                {
                    break;
                }

                //printf("Done\n");

                break;
            }
            case 1: // ColorCode
            {
                /*
                blocks[index].print(buf);
                printf("Sending CC: %s\n", buf);
                */

                // Topic
                err = str_to_msg("CC", &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                err = send_int_as_hex(blocks[index].signature, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].x, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].y, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].width, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].height, publisher, true);
                if (err != 0)
                {
                    break;
                }

                err = sprintf(buf, "%d", blocks[index].angle);
                if (err < 0)
                {
                    break;
                }
                err = str_to_msg(buf, &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, 0);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                //printf("Done\n");

                break;
            }
        }
    }
    i++;
  }
  printf("Exiting\n");
  pixy_close();

    zmq_close(publisher);
    zmq_term(context);

}
<commit_msg>report frame start/end and fram number<commit_after>//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#define ZMQ_PUB_PORT "tcp://*:7580"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include "pixy.h"

#include "zmq.h"

#define BLOCK_BUFFER_SIZE    25

// Pixy Block buffer //
struct Block blocks[BLOCK_BUFFER_SIZE];

static bool run_flag = true;

void handle_SIGINT(int unused)
{
  // On CTRL+C - abort! //

  run_flag = false;
}

int str_to_msg(char* send, zmq_msg_t* msg)
{
    int err;
    int len = strlen(send);
    err = zmq_msg_init_size(msg, len);
    if (err != 0){
        printf("ERROR: zmq_msg_init_size failed with %s\n", zmq_strerror(zmq_errno()));
        return err;
    }
    memcpy(zmq_msg_data(msg), send, len);
    return 0;
}

int send_int_as_hex(uint16_t send, void* publisher, bool more)
{
    char buf[8];
    int err;
    zmq_msg_t msg;

    err = sprintf(buf, "0x%04x", send);
    if (err < 0)
    {
        printf("ERROR: Failed to format message\n");
        return -1;
    }
    err = str_to_msg(buf, &msg);
    if (err != 0)
    {
        zmq_msg_close(&msg);
        return err;
    }
    if (more)
    {
        err = zmq_send(publisher, &msg, ZMQ_SNDMORE);
    }
    else
    {
        err = zmq_send(publisher, &msg, 0);
    }
    zmq_msg_close(&msg);
    if (err != 0)
    {
        printf("ERROR: zmq_send failed with %s\n", zmq_strerror(zmq_errno()));
        return err;
    }
    return 0;
}

int main(int argc, char * argv[])
{
	// ZMQ variables
	int err;
	void* context = zmq_init(1);
	void* publisher = zmq_socket(context, ZMQ_PUB);

	// Prepare ZMQ context and publisher
	err = zmq_bind(publisher, ZMQ_PUB_PORT);
	if(err != 0){
		printf("zmq_bind failed with %s\n", zmq_strerror(zmq_errno()));
		zmq_close(publisher);
		zmq_term(context);
		exit(1);
	}

    zmq_msg_t msgpart;

// Pixy example below
  int      i = 0;
  int      index;
  int      blocks_copied;
  int      pixy_init_status;
  char     buf[128];

  // Catch CTRL+C (SIGINT) signals //
  signal(SIGINT, handle_SIGINT);

  printf("Hello Pixy:\n libpixyusb Version: %s\n", __LIBPIXY_VERSION__);

  // Connect to Pixy //
  pixy_init_status = pixy_init();

  // Was there an error initializing pixy? //
  if(!pixy_init_status == 0)
  {
    // Error initializing Pixy //
    printf("pixy_init(): ");
    pixy_error(pixy_init_status);

    return pixy_init_status;
  }

  // Request Pixy firmware version //
  {
    uint16_t major;
    uint16_t minor;
    uint16_t build;
    int      return_value;

    return_value = pixy_get_firmware_version(&major, &minor, &build);

    if (return_value) {
      // Error //
      printf("Failed to retrieve Pixy firmware version. ");
      pixy_error(return_value);
		zmq_close(publisher);
		zmq_term(context);

      return return_value;
    } else {
      // Success //
      printf(" Pixy Firmware Version: %d.%d.%d\n", major, minor, build);
    }
  }

#if 0
  // Pixy Command Examples //
  {
    int32_t response;
    int     return_value;

    // Execute remote procedure call "cam_setAWB" with one output (host->pixy) parameter (Value = 1)
    //
    //   Parameters:                 Notes:
    //
    //   pixy_command("cam_setAWB",  String identifier for remote procedure
    //                        0x01,  Length (in bytes) of first output parameter
    //                           1,  Value of first output parameter
    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
    //                   &response,  Pointer to memory address for return value from remote procedure call
    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
    //

    // Enable auto white balance //
    pixy_command("cam_setAWB", UINT8(0x01), END_OUT_ARGS,  &response, END_IN_ARGS);

    // Execute remote procedure call "cam_getAWB" with no output (host->pixy) parameters
    //
    //   Parameters:                 Notes:
    //
    //   pixy_command("cam_setAWB",  String identifier for remote procedure
    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
    //                   &response,  Pointer to memory address for return value from remote procedure call
    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
    //

    // Get auto white balance //
    return_value = pixy_command("cam_getAWB", END_OUT_ARGS, &response, END_IN_ARGS);

    // Set auto white balance back to disabled //
    pixy_command("cam_setAWB", UINT8(0x00), END_OUT_ARGS,  &response, END_IN_ARGS);
  }
#endif

    // Frame grab variables
    unsigned char current_frame[72000]; // ~largest possible given current hardware
    unsigned char *pixels;  //returned pointer to video frame buffer
    int32_t response, fourcc;
    int8_t renderflags;
    int return_value, res;
    uint16_t width, height;
    uint32_t  numPixels;

  printf("Detecting blocks...\n");
  while(run_flag)
  {

/**
 * Disable for now
    // Stop block detection for grabbing frame
    return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);

    response = 0;

    return_value = pixy_command("cam_getFrame",  // String id for remote procedure
                             0x01, 0x21,      // mode 0 = 1280x800 25 fps
                             0x02,   0,        // xoffset
                             0x02,   0,         // yoffset
                             0x02, 320,       // width
                             0x02, 200,       // height
                             0,              // separator
                             &response,      // pointer to mem address for return value
                            &fourcc,         //contrary to docs, the next 5 args are needed
                            &renderflags,
                            &width,
                            &height,
                            &numPixels,
                             &pixels,        // pointer to mem address for returned frame
                             0);
    printf("getFrame returned %d response %d\n", return_value, response);
    printf("returned w %d h %d npix %d\n",width,height,numPixels);

    if (return_value == 0)
    {
        memcpy(&current_frame, pixels,numPixels);
        // TODO push frame over gst
    }

    // Restart block detection
    return_value = pixy_command("run", END_OUT_ARGS, &response, END_IN_ARGS);
    if (return_value != 0)
    {
        printf("ERROR: Restarting block detection failed: return_value %d response %d\n", return_value, response);
        run_flag = false;
        continue;
    }
*/

    // Wait for new blocks to be available //
    if (!pixy_blocks_are_new())
    {
        // Yield
        //sleep(0);
        continue;
    }

    // Get blocks from Pixy //
    blocks_copied = pixy_get_blocks(BLOCK_BUFFER_SIZE, &blocks[0]);

    if(blocks_copied < 0) {
      // Error: pixy_get_blocks //
      printf("pixy_get_blocks(): ");
      pixy_error(blocks_copied);
      continue;
    }

    err = str_to_msg("FRAME", &msgpart);
    if (err != 0)
    {
        zmq_msg_close(&msgpart);
        // TODO: Log errors
        continue;
    }
    err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
    zmq_msg_close(&msgpart);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }
    err = str_to_msg("START", &msgpart);
    if (err != 0)
    {
        zmq_msg_close(&msgpart);
        // TODO: Log errors
        continue;
    }
    err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
    zmq_msg_close(&msgpart);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }
    err = send_int_as_hex(i, publisher, false);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }

    // Display received blocks //
    for(index = 0; index != blocks_copied; ++index)
    {
        switch (blocks[index].type)
        {
            case 0: // Normal
            {
                /*
                blocks[index].print(buf);
                printf("Sending NORM: %s\n", buf);
                */

                // Topic
                err = str_to_msg("NORM", &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                err = send_int_as_hex(blocks[index].signature, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].x, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].y, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].width, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].height, publisher, false);
                if (err != 0)
                {
                    break;
                }

                //printf("Done\n");

                break;
            }
            case 1: // ColorCode
            {
                /*
                blocks[index].print(buf);
                printf("Sending CC: %s\n", buf);
                */

                // Topic
                err = str_to_msg("CC", &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                err = send_int_as_hex(blocks[index].signature, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].x, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].y, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].width, publisher, true);
                if (err != 0)
                {
                    break;
                }
                err = send_int_as_hex(blocks[index].height, publisher, true);
                if (err != 0)
                {
                    break;
                }

                err = sprintf(buf, "%d", blocks[index].angle);
                if (err < 0)
                {
                    break;
                }
                err = str_to_msg(buf, &msgpart);
                if (err != 0)
                {
                    zmq_msg_close(&msgpart);
                    break;
                }
                err = zmq_send(publisher, &msgpart, 0);
                zmq_msg_close(&msgpart);
                if (err != 0)
                {
                    break;
                }

                //printf("Done\n");

                break;
            }
        }
    }

    err = str_to_msg("FRAME", &msgpart);
    if (err != 0)
    {
        zmq_msg_close(&msgpart);
        // TODO: Log errors
        continue;
    }
    err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
    zmq_msg_close(&msgpart);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }
    err = str_to_msg("END", &msgpart);
    if (err != 0)
    {
        zmq_msg_close(&msgpart);
        // TODO: Log errors
        continue;
    }
    err = zmq_send(publisher, &msgpart, ZMQ_SNDMORE);
    zmq_msg_close(&msgpart);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }
    err = send_int_as_hex(i, publisher, false);
    if (err != 0)
    {
        // TODO: Log errors
        continue;
    }

    i++;
  }
  printf("Exiting\n");
  pixy_close();

    zmq_close(publisher);
    zmq_term(context);

}
<|endoftext|>


<commit_before>#pragma once
#include <algorithm>
#include <cctype>
#include <string>

inline bool is_lower(const char ch) {
	return std::islower(ch);
}

inline bool is_upper(const char ch) {
	return std::isupper(ch);
}

inline bool is_alpha(const char ch) {
	return std::isalpha(ch);
}

inline bool is_digit(const char ch) {
	return std::isdigit(ch);
}

template<typename T>
inline T to_upper(T);

template<typename T>
inline T to_lower(T);

template<>
inline char to_upper<char>(const char ch) {
	return is_lower(ch) ? ch ^ 32 : ch;
}

template<>
inline char to_lower<char>(const char ch) {
	return is_upper(ch) ? ch ^ 32 : ch;
}

template<>
inline std::string to_upper<std::string>(std::string s) {
	std::transform(s.begin(), s.end(), s.begin(), to_upper<char>);
	return s;
}

template<>
inline std::string to_lower<std::string>(std::string s) {
	std::transform(s.begin(), s.end(), s.begin(), to_lower<char>);
	return s;
}

inline uint8_t to_digit(const char ch)
// get exact numeric value of the corresponding char
// REQUIRE: char from '0' to '9'
{
	return ch - '0';
}

<commit_msg>Added starts_with() and ends_with() method for std::string.<commit_after>#pragma once
#include <algorithm>
#include <cctype>
#include <string>

inline bool is_lower(const char ch) {
	return std::islower(ch);
}

inline bool is_upper(const char ch) {
	return std::isupper(ch);
}

inline bool is_alpha(const char ch) {
	return std::isalpha(ch);
}

inline bool is_digit(const char ch) {
	return std::isdigit(ch);
}

template<typename T>
inline T to_upper(T);

template<typename T>
inline T to_lower(T);

template<>
inline char to_upper<char>(const char ch) {
	return is_lower(ch) ? ch ^ 32 : ch;
}

template<>
inline char to_lower<char>(const char ch) {
	return is_upper(ch) ? ch ^ 32 : ch;
}

template<>
inline std::string to_upper<std::string>(std::string s) {
	std::transform(s.begin(), s.end(), s.begin(), to_upper<char>);
	return s;
}

template<>
inline std::string to_lower<std::string>(std::string s) {
	std::transform(s.begin(), s.end(), s.begin(), to_lower<char>);
	return s;
}

inline uint8_t to_digit(const char ch)
// get exact numeric value of the corresponding char
// REQUIRE: char from '0' to '9'
{
	return ch - '0';
}

bool starts_with(const std::string& s, const std::string& t) {
    return s.length() >= t.length() && s.compare(0, t.length(), t) == 0;
}

bool ends_with(const std::string& s, const std::string& t) {
    return s.length() >= t.length() && s.compare(s.length() - t.length(), t.length(), t) == 0;
}
<|endoftext|>


<commit_before>#include <set>
#include <fstream>

namespace cpr
{
	namespace huffman
	{
		template<typename Freq>	struct  C
		{
			unsigned char character_;
			Freq freq_;
			C* left_, right_;
		};

		template<typename Freq> class Set : public std::set<unsigned char>
		{

		};
	}
}<commit_msg>implemented huffman, not tested yet<commit_after>#include <set>
#include <fstream>
#include <queue>
#include <functional>

namespace cpr
{
	namespace huffman
	{
		template<typename Freq>
		struct  Node
		{
			Node()
				: character_{ 0 }, freq_{ 0 }, left_{ nullptr }, right_{ nullptr }
			{	}

			Node(Node<Freq> const& other)
				: character_{ other.character_ }, freq_{ other.freq_ }, left_{ other.left_ }, right_{other.right_}
			{	}

			unsigned char character_;
			Freq freq_;
			Node* left_, right_;
		};

		template<typename Freq>
		inline bool operator > (Node<Freq> const& lhs, Node<Freq> const& rhs)
		{
			return lhs.freq_ > rhs.freq_;
		}

		template<typename Freq>
		class Set : public std::set<Node<Freq>>
		{

		};

		template<typename Freq>
		Node<Freq> make_huffman_tree(Set<Node<Freq>> const& set)
		{
			using MinPreorityQuee = std::priority_queue < Node<Freq>, std::vector<Node<Freq>>, std::greater<Node<Freq>> > ;
			MinPreorityQuee queue;
			for (auto const& node : set)
				queue.push(node);

			for (int _ = 1; _ != set.size(); ++_)
			{
				Node<Freq> node;
			}
		}
	}
}<|endoftext|>


<commit_before>/*
  ESP8266WiFi.cpp - WiFi library for esp8266

  Copyright (c) 2014 Ivan Grokhotkov. All rights reserved.
  This file is part of the esp8266 core for Arduino environment.

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

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

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "ESP8266WiFi.h"
extern "C" {
#include "c_types.h"
#include "ets_sys.h"
#include "os_type.h"
#include "osapi.h"
#include "mem.h"
#include "user_interface.h"
#include "lwip/opt.h"
#include "lwip/err.h"
#include "lwip/dns.h"
}

extern "C" void esp_schedule();
extern "C" void esp_yield();

ESP8266WiFiClass::ESP8266WiFiClass()
{

}

void ESP8266WiFiClass::mode(WiFiMode m)
{
    ETS_UART_INTR_DISABLE();
    wifi_set_opmode(m);
    ETS_UART_INTR_ENABLE();
}

int ESP8266WiFiClass::begin(const char* ssid)
{
    return begin(ssid, 0);
}

int ESP8266WiFiClass::begin(const char* ssid, const char *passphrase)
{
    if (wifi_get_opmode() == WIFI_AP)
    {
        // turn on AP+STA mode
        mode(WIFI_AP_STA);
    }

    struct station_config conf;
    strcpy(reinterpret_cast<char*>(conf.ssid), ssid);
    if (passphrase)
        strcpy(reinterpret_cast<char*>(conf.password), passphrase);
    else
        *conf.password = 0;

    conf.bssid_set = 0;

    ETS_UART_INTR_DISABLE();
    wifi_station_set_config(&conf);
    wifi_station_connect();
    ETS_UART_INTR_ENABLE();
    wifi_station_dhcpc_start();
    return 0;
}

void ESP8266WiFiClass::config(IPAddress local_ip, IPAddress gateway, IPAddress subnet)
{
    struct ip_info info;
    info.ip.addr = static_cast<uint32_t>(local_ip);
    info.gw.addr = static_cast<uint32_t>(gateway);
    info.netmask.addr = static_cast<uint32_t>(subnet);

    wifi_station_dhcpc_stop();
    wifi_set_ip_info(STATION_IF, &info);
}

int ESP8266WiFiClass::disconnect()
{
    struct station_config conf;
    *conf.ssid = 0;
    *conf.password = 0;
    ETS_UART_INTR_DISABLE();
    wifi_station_set_config(&conf);
    wifi_station_disconnect();
    ETS_UART_INTR_ENABLE();
    return 0;
}

void ESP8266WiFiClass::softAP(const char* ssid)
{
    softAP(ssid, 0);
}

void ESP8266WiFiClass::softAP(const char* ssid, const char* passphrase)
{
    if (wifi_get_opmode() == WIFI_STA)
    {
        // turn on AP+STA mode
        mode(WIFI_AP_STA);
    }

    struct softap_config conf;
    wifi_softap_get_config(&conf);
    strcpy(reinterpret_cast<char*>(conf.ssid), ssid);
    conf.channel = 1;

    if (!passphrase || strlen(passphrase) == 0)
    {
        conf.authmode = AUTH_OPEN;
        *conf.password = 0;
    }
    else
    {
        conf.authmode = AUTH_WPA2_PSK;
        strcpy(reinterpret_cast<char*>(conf.password), passphrase);
    }

    ETS_UART_INTR_DISABLE();
    wifi_softap_set_config(&conf);
    ETS_UART_INTR_ENABLE();
}

uint8_t* ESP8266WiFiClass::macAddress(uint8_t* mac)
{
    wifi_get_macaddr(STATION_IF, mac);
    return mac;
}

uint8_t* softAPmacAddress(uint8_t* mac)
{
    wifi_get_macaddr(SOFTAP_IF, mac);
    return mac;
}

IPAddress ESP8266WiFiClass::localIP()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.ip.addr);
}

IPAddress softAPIP()
{
    struct ip_info ip;
    wifi_get_ip_info(SOFTAP_IF, &ip);
    return IPAddress(ip.ip.addr);
}

IPAddress ESP8266WiFiClass::subnetMask()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.netmask.addr);
}

IPAddress ESP8266WiFiClass::gatewayIP()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.gw.addr);
}

char* ESP8266WiFiClass::SSID()
{
    static struct station_config conf;
    wifi_station_get_config(&conf);
    return reinterpret_cast<char*>(conf.ssid);
}

// uint8_t* ESP8266WiFiClass::BSSID(uint8_t* bssid)
// {
//  uint8_t* _bssid = WiFiDrv::getCurrentBSSID();
//  memcpy(bssid, _bssid, WL_MAC_ADDR_LENGTH);
//     return bssid;
// }

// int32_t ESP8266WiFiClass::RSSI()
// {
//     return WiFiDrv::getCurrentRSSI();
// }

// uint8_t ESP8266WiFiClass::encryptionType()
// {
//     return WiFiDrv::getCurrentEncryptionType();
// }

extern "C"
{
    typedef STAILQ_HEAD(, bss_info) bss_info_head_t;
}

void ESP8266WiFiClass::_scanDone(void* result, int status)
{
    if (status != OK)
    {
        ESP8266WiFiClass::_scanCount = 0;
        ESP8266WiFiClass::_scanResult = 0;
    }
    else
    {

        int i = 0;
        bss_info_head_t* head = reinterpret_cast<bss_info_head_t*>(result);

        for (bss_info* it = STAILQ_FIRST(head); it; it = STAILQ_NEXT(it, next), ++i);
        ESP8266WiFiClass::_scanCount = i;
        if (i == 0)
        {
            ESP8266WiFiClass::_scanResult = 0;
        }
        else
        {
            bss_info* copied_info = new bss_info[i];
            i = 0;
            for (bss_info* it = STAILQ_FIRST(head); it; it = STAILQ_NEXT(it, next), ++i)
            {
                memcpy(copied_info + i, it, sizeof(bss_info));
            }

            ESP8266WiFiClass::_scanResult = copied_info;
        }

    }
    esp_schedule();
}

int8_t ESP8266WiFiClass::scanNetworks()
{
    if (wifi_get_opmode() == WIFI_AP)
    {
        mode(WIFI_AP_STA);
    }
    int status = wifi_station_get_connect_status();
    if (status != STATION_GOT_IP && status != STATION_IDLE)
    {
        disconnect();
    }

    if (ESP8266WiFiClass::_scanResult)
    {
        delete[] reinterpret_cast<bss_info*>(ESP8266WiFiClass::_scanResult);
        ESP8266WiFiClass::_scanResult = 0;
        ESP8266WiFiClass::_scanCount = 0;
    }

    struct scan_config config;
    config.ssid = 0;
    config.bssid = 0;
    config.channel = 0;
    wifi_station_scan(&config, reinterpret_cast<scan_done_cb_t>(&ESP8266WiFiClass::_scanDone));
    esp_yield();
    return ESP8266WiFiClass::_scanCount;
}

void * ESP8266WiFiClass::_getScanInfoByIndex(int i)
{
    if (!ESP8266WiFiClass::_scanResult || i > ESP8266WiFiClass::_scanCount)
    {
        return 0;
    }

    return reinterpret_cast<bss_info*>(ESP8266WiFiClass::_scanResult) + i;
}

const char* ESP8266WiFiClass::SSID(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return 0;

    return reinterpret_cast<const char*>(it->ssid);
}

int32_t ESP8266WiFiClass::RSSI(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return 0;

    return it->rssi;
}

uint8_t ESP8266WiFiClass::encryptionType(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return -1;

    int authmode = it->authmode;
    if (authmode == AUTH_OPEN)
        return ENC_TYPE_NONE;
    if (authmode == AUTH_WEP)
        return ENC_TYPE_WEP;
    if (authmode == AUTH_WPA_PSK || authmode == AUTH_WPA_WPA2_PSK)  // fixme: is this correct?
        return ENC_TYPE_TKIP;
    if (authmode == AUTH_WPA2_PSK)
        return ENC_TYPE_CCMP;
    return -1;
}

uint8_t ESP8266WiFiClass::status()
{
    int status = wifi_station_get_connect_status();

    if (status == STATION_GOT_IP)
      return WL_CONNECTED;
    else if (status == STATION_NO_AP_FOUND)
      return WL_NO_SSID_AVAIL;
    else if (status == STATION_CONNECT_FAIL || status == STATION_WRONG_PASSWORD)
      return WL_CONNECT_FAILED;
    else if (status == STATION_IDLE)
      return WL_IDLE_STATUS;
    else
      return WL_DISCONNECTED;
}

void wifi_dns_found_callback(const char *name, ip_addr_t *ipaddr, void *callback_arg)
{
    if (ipaddr)
        (*reinterpret_cast<IPAddress*>(callback_arg)) = ipaddr->addr;
    esp_schedule(); // resume the hostByName function
}

int ESP8266WiFiClass::hostByName(const char* aHostname, IPAddress& aResult)
{
    ip_addr_t addr;
    aResult = static_cast<uint32_t>(0);
    err_t err = dns_gethostbyname(aHostname, &addr, &wifi_dns_found_callback, &aResult);
    if (err == ERR_OK)
    {
        aResult = addr.addr;
    }
    else if (err == ERR_INPROGRESS)
    {
        esp_yield();
        // will return here when dns_found_callback fires
    }

    return (aResult != 0) ? 1 : 0;
}

void ESP8266WiFiClass::printDiag(Print& p)
{
    const char* modes[] = {"NULL", "STA", "AP", "STA+AP"};
    p.print("Mode: ");
    p.println(modes[wifi_get_opmode()]);

    const char* phymodes[] = {"", "B", "G", "N"};
    p.print("PHY mode: ");
    p.println(phymodes[(int) wifi_get_phy_mode()]);

    p.print("Channel: ");
    p.println(wifi_get_channel());

    p.print("AP id: ");
    p.println(wifi_station_get_current_ap_id());

    p.print("Status: ");
    p.println(wifi_station_get_connect_status());

    p.print("Auto connect: ");
    p.println(wifi_station_get_auto_connect());

    static struct station_config conf;
    wifi_station_get_config(&conf);

    const char* ssid = reinterpret_cast<const char*>(conf.ssid);
    p.print("SSID (");
    p.print(strlen(ssid));
    p.print("): ");
    p.println(ssid);

    const char* passphrase = reinterpret_cast<const char*>(conf.password);
    p.print("Passphrase (");
    p.print(strlen(passphrase));
    p.print("): ");
    p.println(passphrase);

    p.print("BSSID set: ");
    p.println(conf.bssid_set);

}

size_t ESP8266WiFiClass::_scanCount = 0;
void* ESP8266WiFiClass::_scanResult = 0;

ESP8266WiFiClass WiFi;
<commit_msg>Fix encryptionType for AUTH_WPA_WPA2_PSK<commit_after>/*
  ESP8266WiFi.cpp - WiFi library for esp8266

  Copyright (c) 2014 Ivan Grokhotkov. All rights reserved.
  This file is part of the esp8266 core for Arduino environment.

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

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

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "ESP8266WiFi.h"
extern "C" {
#include "c_types.h"
#include "ets_sys.h"
#include "os_type.h"
#include "osapi.h"
#include "mem.h"
#include "user_interface.h"
#include "lwip/opt.h"
#include "lwip/err.h"
#include "lwip/dns.h"
}

extern "C" void esp_schedule();
extern "C" void esp_yield();

ESP8266WiFiClass::ESP8266WiFiClass()
{

}

void ESP8266WiFiClass::mode(WiFiMode m)
{
    ETS_UART_INTR_DISABLE();
    wifi_set_opmode(m);
    ETS_UART_INTR_ENABLE();
}

int ESP8266WiFiClass::begin(const char* ssid)
{
    return begin(ssid, 0);
}

int ESP8266WiFiClass::begin(const char* ssid, const char *passphrase)
{
    if (wifi_get_opmode() == WIFI_AP)
    {
        // turn on AP+STA mode
        mode(WIFI_AP_STA);
    }

    struct station_config conf;
    strcpy(reinterpret_cast<char*>(conf.ssid), ssid);
    if (passphrase)
        strcpy(reinterpret_cast<char*>(conf.password), passphrase);
    else
        *conf.password = 0;

    conf.bssid_set = 0;

    ETS_UART_INTR_DISABLE();
    wifi_station_set_config(&conf);
    wifi_station_connect();
    ETS_UART_INTR_ENABLE();
    wifi_station_dhcpc_start();
    return 0;
}

void ESP8266WiFiClass::config(IPAddress local_ip, IPAddress gateway, IPAddress subnet)
{
    struct ip_info info;
    info.ip.addr = static_cast<uint32_t>(local_ip);
    info.gw.addr = static_cast<uint32_t>(gateway);
    info.netmask.addr = static_cast<uint32_t>(subnet);

    wifi_station_dhcpc_stop();
    wifi_set_ip_info(STATION_IF, &info);
}

int ESP8266WiFiClass::disconnect()
{
    struct station_config conf;
    *conf.ssid = 0;
    *conf.password = 0;
    ETS_UART_INTR_DISABLE();
    wifi_station_set_config(&conf);
    wifi_station_disconnect();
    ETS_UART_INTR_ENABLE();
    return 0;
}

void ESP8266WiFiClass::softAP(const char* ssid)
{
    softAP(ssid, 0);
}

void ESP8266WiFiClass::softAP(const char* ssid, const char* passphrase)
{
    if (wifi_get_opmode() == WIFI_STA)
    {
        // turn on AP+STA mode
        mode(WIFI_AP_STA);
    }

    struct softap_config conf;
    wifi_softap_get_config(&conf);
    strcpy(reinterpret_cast<char*>(conf.ssid), ssid);
    conf.channel = 1;

    if (!passphrase || strlen(passphrase) == 0)
    {
        conf.authmode = AUTH_OPEN;
        *conf.password = 0;
    }
    else
    {
        conf.authmode = AUTH_WPA2_PSK;
        strcpy(reinterpret_cast<char*>(conf.password), passphrase);
    }

    ETS_UART_INTR_DISABLE();
    wifi_softap_set_config(&conf);
    ETS_UART_INTR_ENABLE();
}

uint8_t* ESP8266WiFiClass::macAddress(uint8_t* mac)
{
    wifi_get_macaddr(STATION_IF, mac);
    return mac;
}

uint8_t* softAPmacAddress(uint8_t* mac)
{
    wifi_get_macaddr(SOFTAP_IF, mac);
    return mac;
}

IPAddress ESP8266WiFiClass::localIP()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.ip.addr);
}

IPAddress softAPIP()
{
    struct ip_info ip;
    wifi_get_ip_info(SOFTAP_IF, &ip);
    return IPAddress(ip.ip.addr);
}

IPAddress ESP8266WiFiClass::subnetMask()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.netmask.addr);
}

IPAddress ESP8266WiFiClass::gatewayIP()
{
    struct ip_info ip;
    wifi_get_ip_info(STATION_IF, &ip);
    return IPAddress(ip.gw.addr);
}

char* ESP8266WiFiClass::SSID()
{
    static struct station_config conf;
    wifi_station_get_config(&conf);
    return reinterpret_cast<char*>(conf.ssid);
}

// uint8_t* ESP8266WiFiClass::BSSID(uint8_t* bssid)
// {
//  uint8_t* _bssid = WiFiDrv::getCurrentBSSID();
//  memcpy(bssid, _bssid, WL_MAC_ADDR_LENGTH);
//     return bssid;
// }

// int32_t ESP8266WiFiClass::RSSI()
// {
//     return WiFiDrv::getCurrentRSSI();
// }

// uint8_t ESP8266WiFiClass::encryptionType()
// {
//     return WiFiDrv::getCurrentEncryptionType();
// }

extern "C"
{
    typedef STAILQ_HEAD(, bss_info) bss_info_head_t;
}

void ESP8266WiFiClass::_scanDone(void* result, int status)
{
    if (status != OK)
    {
        ESP8266WiFiClass::_scanCount = 0;
        ESP8266WiFiClass::_scanResult = 0;
    }
    else
    {

        int i = 0;
        bss_info_head_t* head = reinterpret_cast<bss_info_head_t*>(result);

        for (bss_info* it = STAILQ_FIRST(head); it; it = STAILQ_NEXT(it, next), ++i);
        ESP8266WiFiClass::_scanCount = i;
        if (i == 0)
        {
            ESP8266WiFiClass::_scanResult = 0;
        }
        else
        {
            bss_info* copied_info = new bss_info[i];
            i = 0;
            for (bss_info* it = STAILQ_FIRST(head); it; it = STAILQ_NEXT(it, next), ++i)
            {
                memcpy(copied_info + i, it, sizeof(bss_info));
            }

            ESP8266WiFiClass::_scanResult = copied_info;
        }

    }
    esp_schedule();
}

int8_t ESP8266WiFiClass::scanNetworks()
{
    if (wifi_get_opmode() == WIFI_AP)
    {
        mode(WIFI_AP_STA);
    }
    int status = wifi_station_get_connect_status();
    if (status != STATION_GOT_IP && status != STATION_IDLE)
    {
        disconnect();
    }

    if (ESP8266WiFiClass::_scanResult)
    {
        delete[] reinterpret_cast<bss_info*>(ESP8266WiFiClass::_scanResult);
        ESP8266WiFiClass::_scanResult = 0;
        ESP8266WiFiClass::_scanCount = 0;
    }

    struct scan_config config;
    config.ssid = 0;
    config.bssid = 0;
    config.channel = 0;
    wifi_station_scan(&config, reinterpret_cast<scan_done_cb_t>(&ESP8266WiFiClass::_scanDone));
    esp_yield();
    return ESP8266WiFiClass::_scanCount;
}

void * ESP8266WiFiClass::_getScanInfoByIndex(int i)
{
    if (!ESP8266WiFiClass::_scanResult || i > ESP8266WiFiClass::_scanCount)
    {
        return 0;
    }

    return reinterpret_cast<bss_info*>(ESP8266WiFiClass::_scanResult) + i;
}

const char* ESP8266WiFiClass::SSID(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return 0;

    return reinterpret_cast<const char*>(it->ssid);
}

int32_t ESP8266WiFiClass::RSSI(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return 0;

    return it->rssi;
}

uint8_t ESP8266WiFiClass::encryptionType(uint8_t i)
{
    struct bss_info* it = reinterpret_cast<struct bss_info*>(_getScanInfoByIndex(i));
    if (!it)
        return -1;

    int authmode = it->authmode;
    if (authmode == AUTH_OPEN)
        return ENC_TYPE_NONE;
    if (authmode == AUTH_WEP)
        return ENC_TYPE_WEP;
    if (authmode == AUTH_WPA_PSK)
        return ENC_TYPE_TKIP;
    if (authmode == AUTH_WPA2_PSK)
        return ENC_TYPE_CCMP;
    if (authmode == AUTH_WPA_WPA2_PSK)
        return ENC_TYPE_AUTO;
    return -1;
}

uint8_t ESP8266WiFiClass::status()
{
    int status = wifi_station_get_connect_status();

    if (status == STATION_GOT_IP)
      return WL_CONNECTED;
    else if (status == STATION_NO_AP_FOUND)
      return WL_NO_SSID_AVAIL;
    else if (status == STATION_CONNECT_FAIL || status == STATION_WRONG_PASSWORD)
      return WL_CONNECT_FAILED;
    else if (status == STATION_IDLE)
      return WL_IDLE_STATUS;
    else
      return WL_DISCONNECTED;
}

void wifi_dns_found_callback(const char *name, ip_addr_t *ipaddr, void *callback_arg)
{
    if (ipaddr)
        (*reinterpret_cast<IPAddress*>(callback_arg)) = ipaddr->addr;
    esp_schedule(); // resume the hostByName function
}

int ESP8266WiFiClass::hostByName(const char* aHostname, IPAddress& aResult)
{
    ip_addr_t addr;
    aResult = static_cast<uint32_t>(0);
    err_t err = dns_gethostbyname(aHostname, &addr, &wifi_dns_found_callback, &aResult);
    if (err == ERR_OK)
    {
        aResult = addr.addr;
    }
    else if (err == ERR_INPROGRESS)
    {
        esp_yield();
        // will return here when dns_found_callback fires
    }

    return (aResult != 0) ? 1 : 0;
}

void ESP8266WiFiClass::printDiag(Print& p)
{
    const char* modes[] = {"NULL", "STA", "AP", "STA+AP"};
    p.print("Mode: ");
    p.println(modes[wifi_get_opmode()]);

    const char* phymodes[] = {"", "B", "G", "N"};
    p.print("PHY mode: ");
    p.println(phymodes[(int) wifi_get_phy_mode()]);

    p.print("Channel: ");
    p.println(wifi_get_channel());

    p.print("AP id: ");
    p.println(wifi_station_get_current_ap_id());

    p.print("Status: ");
    p.println(wifi_station_get_connect_status());

    p.print("Auto connect: ");
    p.println(wifi_station_get_auto_connect());

    static struct station_config conf;
    wifi_station_get_config(&conf);

    const char* ssid = reinterpret_cast<const char*>(conf.ssid);
    p.print("SSID (");
    p.print(strlen(ssid));
    p.print("): ");
    p.println(ssid);

    const char* passphrase = reinterpret_cast<const char*>(conf.password);
    p.print("Passphrase (");
    p.print(strlen(passphrase));
    p.print("): ");
    p.println(passphrase);

    p.print("BSSID set: ");
    p.println(conf.bssid_set);

}

size_t ESP8266WiFiClass::_scanCount = 0;
void* ESP8266WiFiClass::_scanResult = 0;

ESP8266WiFiClass WiFi;
<|endoftext|>


<commit_before>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file robust_multi_sensor_gaussian_filter.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <fl/util/meta.hpp>
#include <fl/util/profiling.hpp>
#include <fl/util/traits.hpp>

#include <fl/model/observation/robust_multi_sensor_feature_obsrv_model.hpp>

#include <fl/filter/gaussian/update_policy/sigma_point_update_policy.hpp>
#include <fl/filter/gaussian/update_policy/sigma_point_additive_update_policy.hpp>
#include <fl/filter/gaussian/update_policy/sigma_point_additive_uncorrelated_update_policy.hpp>
#include <fl/filter/gaussian/prediction_policy/sigma_point_prediction_policy.hpp>
#include <fl/filter/gaussian/prediction_policy/sigma_point_additive_prediction_policy.hpp>

#include <fl/filter/gaussian/multi_sensor_gaussian_filter.hpp>

namespace fl
{

// Forward delcaration
template<
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
> class RobustMultiSensorGaussianFilter;

/**
 * \internal
 * \ingroup nonlinear_gaussian_filter
 *
 * Traits of the robust multi sensor GaussianFilter based on quadrature
 * with customizable policies, i.e implementations of the time and measurement
 * updates.
 */
template <
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
>
struct Traits<
           RobustMultiSensorGaussianFilter<
               StateTransitionFunction, JointObsrvModel, Quadrature>>
{
    typedef typename StateTransitionFunction::State State;
    typedef typename StateTransitionFunction::Input Input;
    typedef typename JointObsrvModel::Obsrv Obsrv;
    typedef Gaussian<State> Belief;
};

/**
 * \ingroup nonlinear_gaussian_filter
 */
template<
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
>
class RobustMultiSensorGaussianFilter
    : public FilterInterface<
                RobustMultiSensorGaussianFilter<
                    StateTransitionFunction, JointObsrvModel, Quadrature>>
{
public:
    typedef typename StateTransitionFunction::State State;
    typedef typename StateTransitionFunction::Input Input;
    typedef typename JointObsrvModel::Obsrv Obsrv;
    typedef Gaussian<State> Belief;

private:
    /** \cond internal */

    // Get the original local model types
    enum : signed int { ModelCount = JointObsrvModel::ModelCount };
    typedef typename JointObsrvModel::LocalModel PlainLocalModel;

    // Define local feature observation model
    typedef RobustMultiSensorFeatureObsrvModel<
                PlainLocalModel, ModelCount
            > FeatureObsrvModel;

    // Define robust joint feature observation model
    typedef JointObservationModel<
                MultipleOf<FeatureObsrvModel, ModelCount>
            > RobustJointFeatureObsrvModel;
    /**
     * \brief Internal generic multi-sensor GaussianFilter for nonlinear
     *        problems
     */
    typedef MultiSensorGaussianFilter<
                StateTransitionFunction,
                RobustJointFeatureObsrvModel,
                Quadrature
            > InternalMultiSensorGaussianFilter;

    /** \endcond */

public:
    /**
     * \brief Creates a RobustGaussianFilter
     */
    RobustMultiSensorGaussianFilter(
        const StateTransitionFunction& process_model,
        const JointObsrvModel& joint_obsrv_model,
        const Quadrature& quadrature)
        : joint_obsrv_model_(joint_obsrv_model),
          multi_sensor_gaussian_filter_(
              process_model,
              RobustJointFeatureObsrvModel(
                  FeatureObsrvModel(joint_obsrv_model_.local_obsrv_model(),
                                    joint_obsrv_model_.count_local_models()),
                  joint_obsrv_model_.count_local_models()),
              quadrature)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~RobustMultiSensorGaussianFilter() { }

    /**
     * \copydoc FilterInterface::predict
     */
    virtual void predict(const Belief& prior_belief,
                         const Input& input,
                         Belief& predicted_belief)
    {
        multi_sensor_gaussian_filter_.predict(
            prior_belief,
            input,
            predicted_belief);
    }

    /**
     * \copydoc FilterInterface::update
     */
    virtual void update(const Belief& predicted_belief,
                        const Obsrv& y,
                        Belief& posterior_belief)
    {
        typedef typename PlainLocalModel::Obsrv PlainObsrv;
        typedef typename PlainLocalModel::BodyObsrvModel::Noise BodyNoise;
        typedef typename RobustJointFeatureObsrvModel::Obsrv JointFeatureObsrv;

        auto joint_feature_y =
            JointFeatureObsrv(
                joint_feature_model()
                    .obsrv_dimension());
        joint_feature_y.setZero();

        auto local_body_noise_distr =
            Gaussian<BodyNoise>(
                obsrv_model()
                    .local_obsrv_model()
                    .body_model()
                    .noise_dimension());

        auto h = [&](const State& x, const BodyNoise& w)
        {
           return joint_obsrv_model_
                        .local_obsrv_model()
                        .body_model()
                        .observation(x, w);
        };

        enum : signed int
        {
            NumberOfPoints =
                Quadrature::number_of_points(
                    JoinSizes<
                        SizeOf<State>::Value,
                        SizeOf<BodyNoise>::Value
                    >::Value)
        };

        PointSet<State, NumberOfPoints> X;
        PointSet<BodyNoise, NumberOfPoints> R;
        PointSet<PlainObsrv, NumberOfPoints> Z;

        multi_sensor_gaussian_filter_
           .quadrature()
           .transform_to_points(predicted_belief, local_body_noise_distr, X, R);

        auto W = X.covariance_weights_vector().asDiagonal();

        auto y_mean = typename FirstMomentOf<PlainObsrv>::Type();
        auto y_cov = typename SecondMomentOf<PlainObsrv>::Type();

        auto& local_obsrv_model = obsrv_model().local_obsrv_model();
        auto& local_feature_model = joint_feature_model().local_obsrv_model();

        local_feature_model.mean_state(predicted_belief.mean());

        const int local_obsrv_dim = local_obsrv_model.obsrv_dimension();
        const int local_feature_dim = local_feature_model.obsrv_dimension();
        const int sensor_count = joint_obsrv_model_.count_local_models();

        low_level_obsrv_bg.setZero(sensor_count, 1);
        low_level_obsrv_fg.setZero(sensor_count, 1);
        low_level_obsrv_nan.setZero(sensor_count, 1);

        mean_obsrv.setZero(sensor_count);

        INIT_PROFILING
        // compute body_tail_obsrv_model parameters
        for (int i = 0; i < sensor_count; ++i)
        {
            if (!std::isfinite(y(i)))
            {
                low_level_obsrv_nan(i) = 0.25;

                joint_feature_y(i * local_feature_dim) =
                    std::numeric_limits<Real>::quiet_NaN();
                continue;
            }

            joint_obsrv_model_.local_obsrv_model().body_model().id(i);
            multi_sensor_gaussian_filter_
                .quadrature()
                .propagate_points(h, X, R, Z);

            y_mean = Z.mean();
            mean_obsrv(i) = y_mean(0);
            //! \todo BG changes
            if (!std::isfinite(y_mean(0)))
            {
                low_level_obsrv_bg(i) = 0.50;

                joint_feature_y(i * local_feature_dim) =
                    std::numeric_limits<Real>::infinity();

                continue;
            }
            auto Z_c = Z.centered_points();
            y_cov = (Z_c * W * Z_c.transpose());

            // set the current sensor's parameter
            local_feature_model.body_moments(y_mean, y_cov, i);

            auto feature = local_feature_model.feature_obsrv(
                        y.middleRows(i * local_obsrv_dim, local_obsrv_dim));

            joint_feature_y.middleRows(i * local_feature_dim, local_feature_dim) =
                    feature;

            low_level_obsrv_fg(i) = 0.75;

//            std::cout << "mean: " << y_mean
//                      << "    std: " << std::sqrt(y_cov(0,0))
//                      << "   y - mean: " << y(i) - y_mean(0)
//                      << "   phi: " << feature.transpose() << std::endl;

        }
        MEASURE("local feature computation");

        multi_sensor_gaussian_filter_
            .update(predicted_belief, joint_feature_y, posterior_belief);
    }

    Eigen::VectorXd mean_obsrv;

public: /* factory functions */
    virtual Belief create_belief() const
    {
        auto belief = multi_sensor_gaussian_filter_.create_belief();
        return belief; // RVO
    }

public: /* accessors & mutators */
    StateTransitionFunction& process_model()
    {
        return multi_sensor_gaussian_filter_.process_model();
    }

    JointObsrvModel& obsrv_model()
    {
        return joint_obsrv_model_;
    }

    const StateTransitionFunction& process_model() const
    {
        return multi_sensor_gaussian_filter_.process_model();
    }

    const JointObsrvModel& obsrv_model() const
    {
        return joint_obsrv_model_;
    }

    std::string name() const override
    {
        return "RobustMultiSensorGaussianFilter<"
                + this->list_arguments(multi_sensor_gaussian_filter_.name())
                + ">";
    }

    std::string description() const override
    {
        return "Robust multi-sensor GaussianFilter with"
                + this->list_descriptions(
                      multi_sensor_gaussian_filter_.description());
    }

protected:
    RobustJointFeatureObsrvModel& joint_feature_model()
    {
        return multi_sensor_gaussian_filter_.obsrv_model();
    }

protected:
    /** \cond internal */
    JointObsrvModel joint_obsrv_model_;
    InternalMultiSensorGaussianFilter multi_sensor_gaussian_filter_;
    /** \endcond */

public:
    Eigen::VectorXd low_level_obsrv_bg;
    Eigen::VectorXd low_level_obsrv_fg;
    Eigen::VectorXd low_level_obsrv_nan;
};

}
<commit_msg>feature computation bug fix<commit_after>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file robust_multi_sensor_gaussian_filter.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <fl/util/meta.hpp>
#include <fl/util/profiling.hpp>
#include <fl/util/traits.hpp>

#include <fl/model/observation/robust_multi_sensor_feature_obsrv_model.hpp>

#include <fl/filter/gaussian/update_policy/sigma_point_update_policy.hpp>
#include <fl/filter/gaussian/update_policy/sigma_point_additive_update_policy.hpp>
#include <fl/filter/gaussian/update_policy/sigma_point_additive_uncorrelated_update_policy.hpp>
#include <fl/filter/gaussian/prediction_policy/sigma_point_prediction_policy.hpp>
#include <fl/filter/gaussian/prediction_policy/sigma_point_additive_prediction_policy.hpp>

#include <fl/filter/gaussian/multi_sensor_gaussian_filter.hpp>

namespace fl
{

// Forward delcaration
template<
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
> class RobustMultiSensorGaussianFilter;

/**
 * \internal
 * \ingroup nonlinear_gaussian_filter
 *
 * Traits of the robust multi sensor GaussianFilter based on quadrature
 * with customizable policies, i.e implementations of the time and measurement
 * updates.
 */
template <
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
>
struct Traits<
           RobustMultiSensorGaussianFilter<
               StateTransitionFunction, JointObsrvModel, Quadrature>>
{
    typedef typename StateTransitionFunction::State State;
    typedef typename StateTransitionFunction::Input Input;
    typedef typename JointObsrvModel::Obsrv Obsrv;
    typedef Gaussian<State> Belief;
};

/**
 * \ingroup nonlinear_gaussian_filter
 */
template<
    typename StateTransitionFunction,
    typename JointObsrvModel,
    typename Quadrature
>
class RobustMultiSensorGaussianFilter
    : public FilterInterface<
                RobustMultiSensorGaussianFilter<
                    StateTransitionFunction, JointObsrvModel, Quadrature>>
{
public:
    typedef typename StateTransitionFunction::State State;
    typedef typename StateTransitionFunction::Input Input;
    typedef typename JointObsrvModel::Obsrv Obsrv;
    typedef Gaussian<State> Belief;

private:
    /** \cond internal */

    // Get the original local model types
    enum : signed int { ModelCount = JointObsrvModel::ModelCount };
    typedef typename JointObsrvModel::LocalModel PlainLocalModel;

    // Define local feature observation model
    typedef RobustMultiSensorFeatureObsrvModel<
                PlainLocalModel, ModelCount
            > FeatureObsrvModel;

    // Define robust joint feature observation model
    typedef JointObservationModel<
                MultipleOf<FeatureObsrvModel, ModelCount>
            > RobustJointFeatureObsrvModel;
    /**
     * \brief Internal generic multi-sensor GaussianFilter for nonlinear
     *        problems
     */
    typedef MultiSensorGaussianFilter<
                StateTransitionFunction,
                RobustJointFeatureObsrvModel,
                Quadrature
            > InternalMultiSensorGaussianFilter;

    /** \endcond */

public:
    /**
     * \brief Creates a RobustGaussianFilter
     */
    RobustMultiSensorGaussianFilter(
        const StateTransitionFunction& process_model,
        const JointObsrvModel& joint_obsrv_model,
        const Quadrature& quadrature)
        : joint_obsrv_model_(joint_obsrv_model),
          multi_sensor_gaussian_filter_(
              process_model,
              RobustJointFeatureObsrvModel(
                  FeatureObsrvModel(joint_obsrv_model_.local_obsrv_model(),
                                    joint_obsrv_model_.count_local_models()),
                  joint_obsrv_model_.count_local_models()),
              quadrature)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~RobustMultiSensorGaussianFilter() { }

    /**
     * \copydoc FilterInterface::predict
     */
    virtual void predict(const Belief& prior_belief,
                         const Input& input,
                         Belief& predicted_belief)
    {
        multi_sensor_gaussian_filter_.predict(
            prior_belief,
            input,
            predicted_belief);
    }

    /**
     * \copydoc FilterInterface::update
     */
    virtual void update(const Belief& predicted_belief,
                        const Obsrv& y,
                        Belief& posterior_belief)
    {
        typedef typename PlainLocalModel::Obsrv PlainObsrv;
        typedef typename PlainLocalModel::BodyObsrvModel::Noise BodyNoise;
        typedef typename RobustJointFeatureObsrvModel::Obsrv JointFeatureObsrv;

        auto joint_feature_y =
            JointFeatureObsrv(
                joint_feature_model()
                    .obsrv_dimension());
        joint_feature_y.setZero();

        auto local_body_noise_distr =
            Gaussian<BodyNoise>(
                obsrv_model()
                    .local_obsrv_model()
                    .body_model()
                    .noise_dimension());

        auto h = [&](const State& x, const BodyNoise& w)
        {
           return joint_obsrv_model_
                        .local_obsrv_model()
                        .body_model()
                        .observation(x, w);
        };

        enum : signed int
        {
            NumberOfPoints =
                Quadrature::number_of_points(
                    JoinSizes<
                        SizeOf<State>::Value,
                        SizeOf<BodyNoise>::Value
                    >::Value)
        };

        PointSet<State, NumberOfPoints> X;
        PointSet<BodyNoise, NumberOfPoints> R;
        PointSet<PlainObsrv, NumberOfPoints> Z;

        multi_sensor_gaussian_filter_
           .quadrature()
           .transform_to_points(predicted_belief, local_body_noise_distr, X, R);

        auto W = X.covariance_weights_vector().asDiagonal();

        auto y_mean = typename FirstMomentOf<PlainObsrv>::Type();
        auto y_cov = typename SecondMomentOf<PlainObsrv>::Type();

        auto& local_obsrv_model = obsrv_model().local_obsrv_model();
        auto& local_feature_model = joint_feature_model().local_obsrv_model();

        local_feature_model.mean_state(predicted_belief.mean());

        const int local_obsrv_dim = local_obsrv_model.obsrv_dimension();
        const int local_feature_dim = local_feature_model.obsrv_dimension();
        const int sensor_count = joint_obsrv_model_.count_local_models();

        low_level_obsrv_bg.setZero(sensor_count, 1);
        low_level_obsrv_fg.setZero(sensor_count, 1);
        low_level_obsrv_nan.setZero(sensor_count, 1);

        mean_obsrv.setZero(sensor_count);

        // compute body_tail_obsrv_model parameters
        for (int i = 0; i < sensor_count; ++i)
        {
            if (!std::isfinite(y(i)))
            {
                low_level_obsrv_nan(i) = 0.25;

                joint_feature_y(i * local_feature_dim) =
                    std::numeric_limits<Real>::quiet_NaN();
                continue;
            }

//            joint_obsrv_model_.local_obsrv_model().body_model().id(i);
            local_feature_model.id(i);
            multi_sensor_gaussian_filter_
                .quadrature()
                .propagate_points(h, X, R, Z);

            y_mean = Z.mean();
            mean_obsrv(i) = y_mean(0);
            //! \todo BG changes
            if (!std::isfinite(y_mean(0)))
            {
                low_level_obsrv_bg(i) = 0.50;

                joint_feature_y(i * local_feature_dim) =
                    std::numeric_limits<Real>::infinity();

                continue;
            }
            auto Z_c = Z.centered_points();
            y_cov = (Z_c * W * Z_c.transpose());

            // set the current sensor's parameter
            local_feature_model.body_moments(y_mean, y_cov, i);

            local_feature_model.id(i);
            auto feature = local_feature_model.feature_obsrv(
                        y.middleRows(i * local_obsrv_dim, local_obsrv_dim));

            joint_feature_y.middleRows(i * local_feature_dim, local_feature_dim) =
                    feature;

            low_level_obsrv_fg(i) = 0.75;

        }

        multi_sensor_gaussian_filter_
            .update(predicted_belief, joint_feature_y, posterior_belief);
    }

    Eigen::VectorXd mean_obsrv;

public: /* factory functions */
    virtual Belief create_belief() const
    {
        auto belief = multi_sensor_gaussian_filter_.create_belief();
        return belief; // RVO
    }

public: /* accessors & mutators */
    StateTransitionFunction& process_model()
    {
        return multi_sensor_gaussian_filter_.process_model();
    }

    JointObsrvModel& obsrv_model()
    {
        return joint_obsrv_model_;
    }

    const StateTransitionFunction& process_model() const
    {
        return multi_sensor_gaussian_filter_.process_model();
    }

    const JointObsrvModel& obsrv_model() const
    {
        return joint_obsrv_model_;
    }

    std::string name() const override
    {
        return "RobustMultiSensorGaussianFilter<"
                + this->list_arguments(multi_sensor_gaussian_filter_.name())
                + ">";
    }

    std::string description() const override
    {
        return "Robust multi-sensor GaussianFilter with"
                + this->list_descriptions(
                      multi_sensor_gaussian_filter_.description());
    }

protected:
    RobustJointFeatureObsrvModel& joint_feature_model()
    {
        return multi_sensor_gaussian_filter_.obsrv_model();
    }

protected:
    /** \cond internal */
    JointObsrvModel joint_obsrv_model_;
    InternalMultiSensorGaussianFilter multi_sensor_gaussian_filter_;
    /** \endcond */

public:
    Eigen::VectorXd low_level_obsrv_bg;
    Eigen::VectorXd low_level_obsrv_fg;
    Eigen::VectorXd low_level_obsrv_nan;
};

}
<|endoftext|>