<commit_before>//*******************************************************************
//glfont2.cpp -- glFont Version 2.0 implementation
//Copyright (c) 1998-2002 Brad Fish
//See glfont.html for terms of use
//May 14, 2002
//*******************************************************************

//STL headers
#include <string>
#include <utility>
#include <iostream>
#include <fstream>
using namespace std;

//OpenGL headers
#ifdef _WINDOWS
#include <windows.h>
#endif
#include <GL/gl.h>

//glFont header
#include "glfont2.h"
using namespace glfont;

//*******************************************************************
//GLFont Class Implementation
//*******************************************************************
GLFont::GLFont ()
{
	//Initialize header to safe state
	header.tex = -1;
	header.tex_width = 0;
	header.tex_height = 0;
	header.start_char = 0;
	header.end_char = 0;
	header.chars = NULL;
}
//*******************************************************************
GLFont::~GLFont ()
{
	//Destroy the font
	Destroy();
}
//*******************************************************************
bool GLFont::Create (const char *file_name, int tex)
{
	ifstream input;
	int num_chars, num_tex_bytes;
	char *tex_bytes;

	//Destroy the old font if there was one, just to be safe
	Destroy();

	//Open input file
	input.open(file_name, ios::in | ios::binary);
	if (!input)
		return false;

	//Read the header from file
	input.read((char *)&header, sizeof(header));
	header.tex = tex;

	//Allocate space for character array
	num_chars = header.end_char - header.start_char + 1;
	if ((header.chars = new GLFontChar[num_chars]) == NULL)
		return false;

	//Read character array
	for(int i=0; i < num_chars; i++)
		input.read((char *)&header.chars[i], sizeof(GLFontChar));

	//Read texture pixel data
	num_tex_bytes = header.tex_width * header.tex_height * 2;
	tex_bytes = new char[num_tex_bytes];
	input.read(tex_bytes, num_tex_bytes);

	//Create OpenGL texture
	glBindTexture(GL_TEXTURE_2D, tex);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
	glTexImage2D(GL_TEXTURE_2D, 0, 2, header.tex_width,
		header.tex_height, 0, GL_LUMINANCE_ALPHA, GL_UNSIGNED_BYTE,
		(void *)tex_bytes);

	//Free texture pixels memory
	delete[] tex_bytes;

	//Close input file
	input.close();

	//Return successfully

	cout << "Start: " << header.start_char << ", End: " << header.end_char << ", Chars: " << header.chars << endl;
	cout << "Height: " << header.tex_height << ", Width: " << header.tex_width << endl;
	//Read character array
	for(int i=header.start_char; i < header.end_char; i++)
	{
		GLFontChar c = header.chars[i - header.start_char];
		cout << "Char: " << i << ", dx: " << c.dx << ", dy: " << c.dy << endl;
		cout << "ty1: " << c.ty1 << ", ty2: " << c.ty2 << ", tx1: " << c.tx1 << ", tx2: " << c.tx2 << endl;
	}

	return true;
}
//*******************************************************************
bool GLFont::Create (const std::string &file_name, int tex)
{
	return Create(file_name.c_str(), tex);
}
//*******************************************************************
void GLFont::Destroy (void)
{
	//Delete the character array if necessary
	if (header.chars)
	{
		delete[] header.chars;
		header.chars = NULL;
	}
}
//*******************************************************************
void GLFont::GetTexSize (std::pair<int, int> *size)
{
	//Retrieve texture size
	size->first = header.tex_width;
	size->second = header.tex_height;
}
//*******************************************************************
int GLFont::GetTexWidth (void)
{
	//Return texture width
	return header.tex_width;
}
//*******************************************************************
int GLFont::GetTexHeight (void)
{
	//Return texture height
	return header.tex_height;
}
//*******************************************************************
void GLFont::GetCharInterval (std::pair<int, int> *interval)
{
	//Retrieve character interval
	interval->first = header.start_char;
	interval->second = header.end_char;
}
//*******************************************************************
int GLFont::GetStartChar (void)
{
	//Return start character
	return header.start_char;
}
//*******************************************************************
int GLFont::GetEndChar (void)
{
	//Return end character
	return header.end_char;
}
//*******************************************************************
void GLFont::GetCharSize (int c, std::pair<int, int> *size)
{
	//Make sure character is in range
	if (c < header.start_char || c > header.end_char)
	{
		//Not a valid character, so it obviously has no size
		size->first = 0;
		size->second = 0;
	}
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character size
		glfont_char = &header.chars[c - header.start_char];
		size->first = (int)(glfont_char->dx * header.tex_width);
		size->second = (int)(glfont_char->dy *
			header.tex_height);
	}
}
//*******************************************************************
int GLFont::GetCharWidth (int c)
{
	//Make sure in range
	if (c < header.start_char || c > header.end_char)
		return 0;
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character width
		glfont_char = &header.chars[c - header.start_char];
		return (int)(glfont_char->dx * header.tex_width);
	}
}
//*******************************************************************
int GLFont::GetCharHeight (int c)
{
	//Make sure in range
	if (c < header.start_char || c > header.end_char)
		return 0;
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character height
		glfont_char = &header.chars[c - header.start_char];
		return (int)(glfont_char->dy * header.tex_height);
	}
}
//*******************************************************************
void GLFont::Begin (void)
{
	//Bind to font texture
	glBindTexture(GL_TEXTURE_2D, header.tex);
}
//*******************************************************************

//End of file

<commit_msg>fix for 64-bit<commit_after>//*******************************************************************
//glfont2.cpp -- glFont Version 2.0 implementation
//Copyright (c) 1998-2002 Brad Fish
//See glfont.html for terms of use
//May 14, 2002
//*******************************************************************

//STL headers
#include <string>
#include <utility>
#include <iostream>
#include <fstream>
using namespace std;

//OpenGL headers
#ifdef _WINDOWS
#include <windows.h>
#endif
#include <GL/gl.h>

//glFont header
#include "glfont2.h"
using namespace glfont;

//*******************************************************************
//GLFont Class Implementation
//*******************************************************************
GLFont::GLFont ()
{
	//Initialize header to safe state
	header.tex = -1;
	header.tex_width = 0;
	header.tex_height = 0;
	header.start_char = 0;
	header.end_char = 0;
	header.chars = NULL;
}
//*******************************************************************
GLFont::~GLFont ()
{
	//Destroy the font
	Destroy();
}
//*******************************************************************
bool GLFont::Create (const char *file_name, int tex)
{
	ifstream input;
	int num_chars, num_tex_bytes;
	char *tex_bytes;

	//Destroy the old font if there was one, just to be safe
	Destroy();

	//Open input file
	input.open(file_name, ios::in | ios::binary);
	if (!input)
		return false;

	//Read the header from file
	input.read((char *)&header, sizeof(header));
	header.tex = tex;

	//Allocate space for character array
	num_chars = header.end_char - header.start_char + 1;
	if ((header.chars = new GLFontChar[num_chars]) == NULL)
		return false;

	//Read character array
	for(int i=0; i < num_chars; i++)
		input.read((char *)&header.chars[i], sizeof(GLFontChar) - (sizeof(void*) - 4));

	//Read texture pixel data
	num_tex_bytes = header.tex_width * header.tex_height * 2;
	tex_bytes = new char[num_tex_bytes];
	input.read(tex_bytes, num_tex_bytes);

	//Create OpenGL texture
	glBindTexture(GL_TEXTURE_2D, tex);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
	glTexImage2D(GL_TEXTURE_2D, 0, 2, header.tex_width,
		header.tex_height, 0, GL_LUMINANCE_ALPHA, GL_UNSIGNED_BYTE,
		(void *)tex_bytes);

	//Free texture pixels memory
	delete[] tex_bytes;

	//Close input file
	input.close();

	//Return successfully

	cout << "Start: " << header.start_char << ", End: " << header.end_char << ", Chars: " << header.chars << endl;
	cout << "Height: " << header.tex_height << ", Width: " << header.tex_width << endl;
	//Read character array
	for(int i=header.start_char; i < header.end_char; i++)
	{
		GLFontChar c = header.chars[i - header.start_char];
		cout << "Char: " << i << ", dx: " << c.dx << ", dy: " << c.dy << endl;
		cout << "ty1: " << c.ty1 << ", ty2: " << c.ty2 << ", tx1: " << c.tx1 << ", tx2: " << c.tx2 << endl;
	}

	return true;
}
//*******************************************************************
bool GLFont::Create (const std::string &file_name, int tex)
{
	return Create(file_name.c_str(), tex);
}
//*******************************************************************
void GLFont::Destroy (void)
{
	//Delete the character array if necessary
	if (header.chars)
	{
		delete[] header.chars;
		header.chars = NULL;
	}
}
//*******************************************************************
void GLFont::GetTexSize (std::pair<int, int> *size)
{
	//Retrieve texture size
	size->first = header.tex_width;
	size->second = header.tex_height;
}
//*******************************************************************
int GLFont::GetTexWidth (void)
{
	//Return texture width
	return header.tex_width;
}
//*******************************************************************
int GLFont::GetTexHeight (void)
{
	//Return texture height
	return header.tex_height;
}
//*******************************************************************
void GLFont::GetCharInterval (std::pair<int, int> *interval)
{
	//Retrieve character interval
	interval->first = header.start_char;
	interval->second = header.end_char;
}
//*******************************************************************
int GLFont::GetStartChar (void)
{
	//Return start character
	return header.start_char;
}
//*******************************************************************
int GLFont::GetEndChar (void)
{
	//Return end character
	return header.end_char;
}
//*******************************************************************
void GLFont::GetCharSize (int c, std::pair<int, int> *size)
{
	//Make sure character is in range
	if (c < header.start_char || c > header.end_char)
	{
		//Not a valid character, so it obviously has no size
		size->first = 0;
		size->second = 0;
	}
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character size
		glfont_char = &header.chars[c - header.start_char];
		size->first = (int)(glfont_char->dx * header.tex_width);
		size->second = (int)(glfont_char->dy *
			header.tex_height);
	}
}
//*******************************************************************
int GLFont::GetCharWidth (int c)
{
	//Make sure in range
	if (c < header.start_char || c > header.end_char)
		return 0;
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character width
		glfont_char = &header.chars[c - header.start_char];
		return (int)(glfont_char->dx * header.tex_width);
	}
}
//*******************************************************************
int GLFont::GetCharHeight (int c)
{
	//Make sure in range
	if (c < header.start_char || c > header.end_char)
		return 0;
	else
	{
		GLFontChar *glfont_char;

		//Retrieve character height
		glfont_char = &header.chars[c - header.start_char];
		return (int)(glfont_char->dy * header.tex_height);
	}
}
//*******************************************************************
void GLFont::Begin (void)
{
	//Bind to font texture
	glBindTexture(GL_TEXTURE_2D, header.tex);
}
//*******************************************************************

//End of file

<|endoftext|>


<commit_before>#include <iostream>

#include "er_helper.h"

int
main (int argc, char** argv)
{
  RGBDTrajectory traj;
  traj.LoadFromFile(argv[1]);

  RGBDTrajectory out;
  out.data_.push_back(traj.data_[0]);

  for(int i=1; i < traj.data_.size(); i++){
    out.data_.push_back(traj.data_[i]);
    out.data_[i].transformation_ = (out.data_[i-1].transformation_ * out.data_[i].transformation_ );

//    traj.data_[i].transformation_ = traj.data_[i].transformation_.inverse().eval();
  }

  out.SaveToFile(argv[2]);
}
<commit_msg>make lcoal_to_global handle arbitrary start/end points.<commit_after>#include <iostream>

#include "er_helper.h"

int
main (int argc, char** argv)
{
  int start=0;
  int end=-1;

  if(argc >= 4){
    start = atoi(argv[3]);
    end = atoi(argv[4]);
  }

  RGBDTrajectory traj;
  traj.LoadFromFile(argv[1]);

  int i = start;
  RGBDTrajectory out;
  out.data_.push_back(traj.data_[i++]); //Push and inc.

  while( i < traj.data_.size() && (end != -1 || i < end)){
    out.data_.push_back(traj.data_[i]);
    out.data_[i].transformation_ = (out.data_[i-1].transformation_ * out.data_[i].transformation_ );
    i++;
  }

  out.SaveToFile(argv[2]);
}
<|endoftext|>


<commit_before>//
// This file is part of Easylogging++ samples
//
// Demonstration of multithreaded application using pthread
//
// Compile this program using (if using gcc-c++ or intel or clang++):
//     [icpc | g++ | clang++] ./pthread.cpp -o bin/./pthread.cpp.bin -D_ELPP_THREAD_SAFE -std=c++0x -pthread -Wall -Wextra
//
// Revision: 1.1
// @author mkhan3189
//

#include <pthread.h>
#include "easylogging++.h"

_INITIALIZE_EASYLOGGINGPP
class MyHandler : public el::LogDispatchCallback {
public:
    void handle(const el::LogDispatchData*) {
        std::cout << "Test MyHandler " << std::endl;
    }
};

struct Args {
  const char* thrId;
  el::Logger* logger;
}args;

void* write2(void* args){
  struct Args* a = (struct Args*)args;

  char* threadId = (char*)a->thrId;
  el::Logger* logger = (el::Logger*)a->logger;
  CLOG(INFO, "default", "network") << "Triggering network log from default and network";

  LOG(INFO) << "Writing from different function using macro [Thread #" << threadId << "]";

  logger->info("Info log from [Thread #%v]", threadId);
  logger->info("Info log");
  logger->verbose(2, "Verbose test [Thread #%v]", threadId);
  logger->verbose(2, "Verbose test");

  CLOG(INFO, "default", "network") << "Triggering network log from default and network";
  return NULL;
}

void *write(void* thrId){
  char* threadId = (char*)thrId;
  // Following line will be logged with every thread
  LOG(INFO) << "This standard log is written by [Thread #" << threadId << "]";
  // Following line will be logged with every thread only when --v=2 argument
  // is provided, i.e, ./bin/multithread_test.cpp.bin --v=2
  VLOG(2) << "This is verbose level 2 logging from [Thread #" << threadId << "]";

  // Following line will be logged only once from second running thread (which every runs second into
  // this line because of interval 2)
  LOG_EVERY_N(2, WARNING) << "This will be logged only once from thread who every reaches this line first. Currently running from [Thread #" << threadId << "]";

  for (int i = 1; i <= 10; ++i) {
     VLOG_IF(true, 2) << "Verbose condition [Thread #" << threadId << "]";
     VLOG_EVERY_N(2, 3) << "Verbose level 3 log every 4th time. This is at " << i << " from [Thread #" << threadId << "]";
  }

  // Following line will be logged once with every thread because of interval 1
  LOG_EVERY_N(1, INFO) << "This interval log will be logged with every thread, this one is from [Thread #" << threadId << "]";

  LOG_IF(strcmp(threadId, "2") == 0, INFO) << "This log is only for thread 2 and is ran by [Thread #" << threadId << "]";

  // Register 5 vague loggers
  for (int i = 1; i <= 5; ++i) {
     std::stringstream ss;
     ss << "logger" << i;
     el::Logger* logger = el::Loggers::getLogger(ss.str());
     LOG(INFO) << "Registered logger [" << *logger << "] [Thread #" << threadId << "]";
     CLOG(INFO, "default", "network") << "Triggering network log from default and network";
  }
  CLOG(INFO, "logger1") << "Logging using new logger [Thread #" << threadId << "]";
  CLOG(INFO, "no-logger") << "THIS SHOULD SAY LOGGER NOT REGISTERED YET [Thread #" << threadId << "]"; // << -- NOTE THIS!
  CLOG(INFO, "default", "network") << "Triggering network log from default and network";

  el::Logger* logger = el::Loggers::getLogger("default");
  logger->info("Info log from [Thread #%v]", threadId);

  // Check for log counters positions
  for (int i = 1; i <= 50; ++i) {
     LOG_EVERY_N(2, INFO) << "Counter pos: " << ELPP_COUNTER_POS << " [Thread #" << threadId << "]";
  }
  LOG_EVERY_N(2, INFO) << "Counter pos: " << ELPP_COUNTER_POS << " [Thread #" << threadId << "]";
  return NULL;
}

// If you wish you can define your own way to get thread ID
const char* getThreadId_CustomVersion(void) {
    return std::to_string(static_cast<long long>(pthread_self())).c_str();
}

int main(int argc, char** argv)
{
     _START_EASYLOGGINGPP(argc, argv);

      el::Helpers::installLogDispatchCallback<MyHandler>("MyHandler");

     // Your thread ID specification
     el::CustomFormatSpecifier myThreadIdSpecifier("%mythreadId", getThreadId_CustomVersion);
     el::Helpers::installCustomFormatSpecifier(myThreadIdSpecifier);

     el::Loggers::addFlag(el::LoggingFlag::MultiLoggerSupport);

     // Note your %mythreadId or built-in, both are logged
     el::Loggers::reconfigureAllLoggers(el::ConfigurationType::Format, "%datetime %level (%thread | %mythreadId) [%logger] [%func] [%loc] %msg");
     el::Loggers::reconfigureAllLoggers(el::Level::Verbose, el::ConfigurationType::Format, "%datetime %level-%vlevel (%thread | %mythreadId) [%logger] [%func] [%loc] %msg");
     el::Loggers::getLogger("network");

     pthread_t thread1, thread2, thread3, thread4;

     // Create independent threads each of which will execute function
     pthread_create( &thread1, NULL, write, (void*)"1");
     pthread_create( &thread2, NULL, write, (void*)"2");
     pthread_create( &thread3, NULL, write, (void*)"3");

     el::Logger* logger = el::Loggers::getLogger("default");
     args.thrId = "4";
     args.logger = logger;
     pthread_create( &thread4, NULL, write2, (void*)&args);

     pthread_join(thread1, NULL);
     pthread_join(thread2, NULL);
     pthread_join(thread3, NULL);
     pthread_join(thread4, NULL);

#if 1 // Change this to 1 for some serious multiple threads
    int i = 5; // Last one we created was 4 so we dont want to confuse
    const int max = i + 500;
    for (; i <= max; ++i) {
        pthread_t thread;
        std::string s = std::to_string(static_cast<long long>(i));
        if (i % 2 == 0)
            pthread_create( &thread, NULL, write, (void*)s.c_str());
        else {
            args.thrId = s.c_str();
            args.logger = logger;
            pthread_create( &thread, NULL, write2, (void*)&args);
        }
        pthread_join(thread, NULL);
    }
#endif

    exit(0);
}
<commit_msg>disabled pthread serious test as it worries travis<commit_after>//
// This file is part of Easylogging++ samples
//
// Demonstration of multithreaded application using pthread
//
// Compile this program using (if using gcc-c++ or intel or clang++):
//     [icpc | g++ | clang++] ./pthread.cpp -o bin/./pthread.cpp.bin -D_ELPP_THREAD_SAFE -std=c++0x -pthread -Wall -Wextra
//
// Revision: 1.1
// @author mkhan3189
//

#include <pthread.h>
#include "easylogging++.h"

_INITIALIZE_EASYLOGGINGPP
class MyHandler : public el::LogDispatchCallback {
public:
    void handle(const el::LogDispatchData*) {
        std::cout << "Test MyHandler " << std::endl;
    }
};

struct Args {
  const char* thrId;
  el::Logger* logger;
}args;

void* write2(void* args){
  struct Args* a = (struct Args*)args;

  char* threadId = (char*)a->thrId;
  el::Logger* logger = (el::Logger*)a->logger;
  CLOG(INFO, "default", "network") << "Triggering network log from default and network";

  LOG(INFO) << "Writing from different function using macro [Thread #" << threadId << "]";

  logger->info("Info log from [Thread #%v]", threadId);
  logger->info("Info log");
  logger->verbose(2, "Verbose test [Thread #%v]", threadId);
  logger->verbose(2, "Verbose test");

  CLOG(INFO, "default", "network") << "Triggering network log from default and network";
  return NULL;
}

void *write(void* thrId){
  char* threadId = (char*)thrId;
  // Following line will be logged with every thread
  LOG(INFO) << "This standard log is written by [Thread #" << threadId << "]";
  // Following line will be logged with every thread only when --v=2 argument
  // is provided, i.e, ./bin/multithread_test.cpp.bin --v=2
  VLOG(2) << "This is verbose level 2 logging from [Thread #" << threadId << "]";

  // Following line will be logged only once from second running thread (which every runs second into
  // this line because of interval 2)
  LOG_EVERY_N(2, WARNING) << "This will be logged only once from thread who every reaches this line first. Currently running from [Thread #" << threadId << "]";

  for (int i = 1; i <= 10; ++i) {
     VLOG_IF(true, 2) << "Verbose condition [Thread #" << threadId << "]";
     VLOG_EVERY_N(2, 3) << "Verbose level 3 log every 4th time. This is at " << i << " from [Thread #" << threadId << "]";
  }

  // Following line will be logged once with every thread because of interval 1
  LOG_EVERY_N(1, INFO) << "This interval log will be logged with every thread, this one is from [Thread #" << threadId << "]";

  LOG_IF(strcmp(threadId, "2") == 0, INFO) << "This log is only for thread 2 and is ran by [Thread #" << threadId << "]";

  // Register 5 vague loggers
  for (int i = 1; i <= 5; ++i) {
     std::stringstream ss;
     ss << "logger" << i;
     el::Logger* logger = el::Loggers::getLogger(ss.str());
     LOG(INFO) << "Registered logger [" << *logger << "] [Thread #" << threadId << "]";
     CLOG(INFO, "default", "network") << "Triggering network log from default and network";
  }
  CLOG(INFO, "logger1") << "Logging using new logger [Thread #" << threadId << "]";
  CLOG(INFO, "no-logger") << "THIS SHOULD SAY LOGGER NOT REGISTERED YET [Thread #" << threadId << "]"; // << -- NOTE THIS!
  CLOG(INFO, "default", "network") << "Triggering network log from default and network";

  el::Logger* logger = el::Loggers::getLogger("default");
  logger->info("Info log from [Thread #%v]", threadId);

  // Check for log counters positions
  for (int i = 1; i <= 50; ++i) {
     LOG_EVERY_N(2, INFO) << "Counter pos: " << ELPP_COUNTER_POS << " [Thread #" << threadId << "]";
  }
  LOG_EVERY_N(2, INFO) << "Counter pos: " << ELPP_COUNTER_POS << " [Thread #" << threadId << "]";
  return NULL;
}

// If you wish you can define your own way to get thread ID
const char* getThreadId_CustomVersion(void) {
    return std::to_string(static_cast<long long>(pthread_self())).c_str();
}

int main(int argc, char** argv)
{
     _START_EASYLOGGINGPP(argc, argv);

      el::Helpers::installLogDispatchCallback<MyHandler>("MyHandler");

     // Your thread ID specification
     el::CustomFormatSpecifier myThreadIdSpecifier("%mythreadId", getThreadId_CustomVersion);
     el::Helpers::installCustomFormatSpecifier(myThreadIdSpecifier);

     el::Loggers::addFlag(el::LoggingFlag::MultiLoggerSupport);

     // Note your %mythreadId or built-in, both are logged
     el::Loggers::reconfigureAllLoggers(el::ConfigurationType::Format, "%datetime %level (%thread | %mythreadId) [%logger] [%func] [%loc] %msg");
     el::Loggers::reconfigureAllLoggers(el::Level::Verbose, el::ConfigurationType::Format, "%datetime %level-%vlevel (%thread | %mythreadId) [%logger] [%func] [%loc] %msg");
     el::Loggers::getLogger("network");

     pthread_t thread1, thread2, thread3, thread4;

     // Create independent threads each of which will execute function
     pthread_create( &thread1, NULL, write, (void*)"1");
     pthread_create( &thread2, NULL, write, (void*)"2");
     pthread_create( &thread3, NULL, write, (void*)"3");

     el::Logger* logger = el::Loggers::getLogger("default");
     args.thrId = "4";
     args.logger = logger;
     pthread_create( &thread4, NULL, write2, (void*)&args);

     pthread_join(thread1, NULL);
     pthread_join(thread2, NULL);
     pthread_join(thread3, NULL);
     pthread_join(thread4, NULL);

#if 0 // Change this to 1 for some serious multiple threads
    int i = 5; // Last one we created was 4 so we dont want to confuse
    const int max = i + 500;
    for (; i <= max; ++i) {
        pthread_t thread;
        std::string s = std::to_string(static_cast<long long>(i));
        if (i % 2 == 0)
            pthread_create( &thread, NULL, write, (void*)s.c_str());
        else {
            args.thrId = s.c_str();
            args.logger = logger;
            pthread_create( &thread, NULL, write2, (void*)&args);
        }
        pthread_join(thread, NULL);
    }
#endif

    exit(0);
}
<|endoftext|>


<commit_before>#include "foo.h"
#include "readjson.h"

#include <iostream>
#include <nlohmann/json.hpp>
#include <QFile>
#include <QDebug>

#define DATAJSON    "../../json/all.json"

    using namespace std;
using namespace nlohmann;

void to_json(json& j, const Person& p)
{
    j = json{
        {"name", p.getName()},
        {"address", p.getAddress()},
        {"age", p.getAge()}
    };
}

void from_json(const json& j, Person& p)
{
    cout << "test " << __func__ << "===>" << endl;
    p.setName( j.at("name").get<std::string>() );
    p.setAddress( j.at("address").get<std::string>() );
    p.setAge( j.at("age").get<int>() );
}

void test_foo()
{
    cout << "test " << __func__ << "===>" << endl;
    //Person p = {"Alice Smith", "Taipei City", 30};
    Person p;
    p.setName("Alice Smith");
    p.setAddress("Taipei City");
    p.setAge(30);

    // convert **Person** to **json**
    json j = p;
    cout << j << endl;
    // convert **json** to **p2**
    Person p2 = j;

    assert(p == p2);
}

void test_bar()
{
    cout << "test " << __func__ << "===>" << endl;
    // create an object from an object_t value
    json::object_t object_value = { {"one", 1}, {"two", 2} };
    json j_object_t(object_value);
    cout << "t_object_t: " << j_object_t.dump(4) << endl;

    // create an object from std::map
    std::map<std::string, int> c_map
    {
        {"apple", 100}, {"whiskey", 300}, {"tiger", 250}
    };
    json j_map(c_map);
    cout << "c_map: " << j_map.dump(4) << endl;
    // string str = j_map.dump();
    // cout << "str: " << str << endl;
}

void test_weather()
{
    cout << "test " << __func__ << "===>" << endl;
    ifstream inf("../query.json");
    json jj;
    inf >> jj;
    //cout << jj.dump(4) << endl;

    string created = jj["query"]["created"].get<string>();
    cout << "created: " << created << endl;

    map<string, string> condition = jj["query"]["results"]["channel"]["item"]["condition"].get<map<string, string>>();

    condition["hello"] = "world";

    for (map<string,string>::iterator it=condition.begin(); it!=condition.end(); ++it) {
        cout << it->first << " => " << it->second << '\n';
    }

    //cout << "temp:" << condition["temp"] << endl;

    cout << "===== part2 =====\n";
    for (auto it=condition.begin(); it!=condition.end(); ++it) {
        cout << it->first << " => " << it->second << endl;
    }

}

void test_qjson()
{
    cout << "test " << __func__ << "===>" << endl;
    QString fn = DATAJSON;
    ReadJson j(fn);
    if (!j.loadFile()) {
        qWarning() << "fail to load..." << fn;
        return;
    }
    //QJsonObject j = j.getJobject();
    qDebug() << j.getLeafString("query.results.channel.item.condition.text");
    //j["query"].toObject()["results"].toObject()["channel"]...["text"].toString()
}

void test_atxt_by_hpp()
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = DATAJSON;
    if (!QFile::exists(json_file.c_str())) {
        qDebug() << "file not found:" << json_file.c_str();
        return;
    }

    try {
        ifstream inf(json_file);
        json j;
        inf >> j;

        cout << j.at("query").at("results").at("channel").at("item").at("condition").at("text") << endl;
#if 0
        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            for (auto& e: j[it.key()]) {
                string key = it.key();
                cout << key << ": " << e << endl;
            }
        }
#endif
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    } catch (json::out_of_range& e) {
        cout << "out_of_range:" << e.what() << endl;
    }
}

void test_atxt()
{
    cout << "test " << __func__ << "===>" << endl;
    qDebug() << "using Qt json...";
    test_qjson();
    qDebug() << "using json.hpp...";
    test_atxt_by_hpp();
}

void test_obj()
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = "notexist.json";

    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json j;
        inf >> j;

        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            for (auto& e: j[it.key()]) {
                string key = it.key();
                cout << key << ": " << e << endl;
            }
        }
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    }
}

string get_from_id(const string& msgid)
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = "../strdef.json";
    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json jj;
        inf >> jj;

        string id = jj.at("General").at(msgid);
        string foo = jj.at("ar_AE").at(id);

        return foo;
    } catch (json::out_of_range& e) {
       std::cout << "out of range:" << e.what() << '\n';
    }

    return "";
}

void test_addon()
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = "../apps-add-ons.json";
    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json j;
        inf >> j;

        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            json item = (*it);
            cout << item.at("name") << endl
                << item.at("icon") << endl;
        }
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    }
}

void test()
{
    cout << "test " << __func__ << "===>" << endl;
    // test_foo();
    // cout << endl;
    test_bar();
    cout << endl;

    test_weather();

    test_atxt();
    cout << "ok" << endl;

    //test_addon();
}
<commit_msg>update jjtest, try test_obj()<commit_after>#include "foo.h"
#include "readjson.h"

#include <iostream>
#include <nlohmann/json.hpp>
#include <QFile>
#include <QDebug>

#define DATAJSON    "../../json/all.json"

    using namespace std;
using namespace nlohmann;

void to_json(json& j, const Person& p)
{
    j = json{
        {"name", p.getName()},
        {"address", p.getAddress()},
        {"age", p.getAge()}
    };
}

void from_json(const json& j, Person& p)
{
    cout << "test " << __func__ << "===>" << endl;
    p.setName( j.at("name").get<std::string>() );
    p.setAddress( j.at("address").get<std::string>() );
    p.setAge( j.at("age").get<int>() );
}

void test_foo()
{
    cout << "test " << __func__ << "===>" << endl;
    //Person p = {"Alice Smith", "Taipei City", 30};
    Person p;
    p.setName("Alice Smith");
    p.setAddress("Taipei City");
    p.setAge(30);

    // convert **Person** to **json**
    json j = p;
    cout << j << endl;
    // convert **json** to **p2**
    Person p2 = j;

    assert(p == p2);
}

void test_bar()
{
    cout << "test " << __func__ << "===>" << endl;
    // create an object from an object_t value
    json::object_t object_value = { {"one", 1}, {"two", 2} };
    json j_object_t(object_value);
    cout << "t_object_t: " << j_object_t.dump(4) << endl;

    // create an object from std::map
    std::map<std::string, int> c_map
    {
        {"apple", 100}, {"whiskey", 300}, {"tiger", 250}
    };
    json j_map(c_map);
    cout << "c_map: " << j_map.dump(4) << endl;
    // string str = j_map.dump();
    // cout << "str: " << str << endl;
}

void test_weather()
{
    cout << "test " << __func__ << "===>" << endl;
    ifstream inf("../query.json");
    json jj;
    inf >> jj;
    //cout << jj.dump(4) << endl;

    string created = jj["query"]["created"].get<string>();
    cout << "created: " << created << endl;

    map<string, string> condition = jj["query"]["results"]["channel"]["item"]["condition"].get<map<string, string>>();

    condition["hello"] = "world";

    for (map<string,string>::iterator it=condition.begin(); it!=condition.end(); ++it) {
        cout << it->first << " => " << it->second << '\n';
    }

    //cout << "temp:" << condition["temp"] << endl;

    cout << "===== part2 =====\n";
    for (auto it=condition.begin(); it!=condition.end(); ++it) {
        cout << it->first << " => " << it->second << endl;
    }

}

void test_qjson()
{
    cout << "test " << __func__ << "===>" << endl;
    QString fn = DATAJSON;
    ReadJson j(fn);
    if (!j.loadFile()) {
        qWarning() << "fail to load..." << fn;
        return;
    }
    //QJsonObject j = j.getJobject();
    qDebug() << j.getLeafString("query.results.channel.item.condition.text");
    //j["query"].toObject()["results"].toObject()["channel"]...["text"].toString()
}

void test_atxt_by_hpp()
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = DATAJSON;
    if (!QFile::exists(json_file.c_str())) {
        qDebug() << "file not found:" << json_file.c_str();
        return;
    }

    try {
        ifstream inf(json_file);
        json j;
        inf >> j;

        cout << j.at("query").at("results").at("channel").at("item").at("condition").at("text") << endl;
#if 0
        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            for (auto& e: j[it.key()]) {
                string key = it.key();
                cout << key << ": " << e << endl;
            }
        }
#endif
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    } catch (json::out_of_range& e) {
        cout << "out_of_range:" << e.what() << endl;
    }
}

void test_atxt()
{
    cout << "test " << __func__ << "===>" << endl;
    qDebug() << "using Qt json...";
    test_qjson();
    qDebug() << "using json.hpp...";
    test_atxt_by_hpp();
}

void test_obj()
{
    using namespace std;

    cout << "test " << __func__ << "===>" << endl;
    //string json_file = "notexist.json";
    string json_file = "../query.json";

    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json j;
        inf >> j;
        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            for (auto& e: j[it.key()]) {
                string key = it.key();
                cout << key << ": " << e << endl;
            }
        }
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    }
}

string get_from_id(const string& msgid)
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = "../strdef.json";
    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json jj;
        inf >> jj;

        string id = jj.at("General").at(msgid);
        string foo = jj.at("ar_AE").at(id);

        return foo;
    } catch (json::out_of_range& e) {
       std::cout << "out of range:" << e.what() << '\n';
    }

    return "";
}

void test_addon()
{
    cout << "test " << __func__ << "===>" << endl;
    string json_file = "../apps-add-ons.json";
    cout << "read " << json_file << endl;
    try {
        ifstream inf(json_file);
        json j;
        inf >> j;

        for (json::iterator it = j.begin(); it != j.end(); ++it) {
            json item = (*it);
            cout << item.at("name") << endl
                << item.at("icon") << endl;
        }
    } catch (json::parse_error& e) {
        cout << "parse error:" << e.what() << endl;
    }
}

void test()
{
    cout << "test " << __func__ << "===>" << endl;
    // test_foo();
    // cout << endl;
    test_bar();
    cout << endl;

    test_weather();

    test_atxt();
    cout << "ok" << endl;

    test_obj();

    //test_addon();
}
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (ivan.frade@nokia.com)
**
** This file is part of the test suite of the QtSparql module (not yet part of the Qt Toolkit).
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at ivan.frade@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "../testhelpers.h"

#include <tracker-sparql.h>

#include <QtTest/QtTest>
#include <QtSparql/QtSparql>

#include <sys/time.h>
#include <stdio.h>

#define START_BENCHMARK { \
    struct timeval benchmark_tv; \
    gettimeofday(&benchmark_tv, NULL); \
    const long benchmark_start = benchmark_tv.tv_sec * 1000 + (benchmark_tv.tv_usec + 500) / 1000; \
    do

#define END_BENCHMARK(BENCHMARKNAME) while(false); \
        gettimeofday(&benchmark_tv, NULL); \
        const long benchmark_end = benchmark_tv.tv_sec * 1000 + (benchmark_tv.tv_usec + 500) / 1000; \
        char benchmark_tsbuf[80]; \
        qsnprintf(benchmark_tsbuf, sizeof(benchmark_tsbuf), "QSparql_bnechmark_data: %s %lu\n",  \
                  qPrintable(BENCHMARKNAME), benchmark_end - benchmark_start); \
        fwrite(benchmark_tsbuf, qstrnlen(benchmark_tsbuf, sizeof(benchmark_tsbuf)), 1, stderr); \
    }

#define NO_QUERIES 100

class tst_QSparqlBenchmark : public QObject
{
    Q_OBJECT

public:
    tst_QSparqlBenchmark();
    virtual ~tst_QSparqlBenchmark();

public slots:
    void initTestCase();
    void cleanupTestCase();
    void init();
    void cleanup();

private slots:
    // Actual benchmarks
    void queryBenchmark();
    void queryBenchmark_data();

    void dataReadingBenchmark();
    void dataReadingBenchmark_data();

    // Reference benchmarks
    void queryWithLibtrackerSparql();
    void queryWithLibtrackerSparql_data();

    void queryWithLibtrackerSparqlInThread();
    void queryWithLibtrackerSparqlInThread_data();

    void threadCreatingOverhead();
};

namespace {
    // Query strings

    // Test data for running this can be found in the tracker project.
const QString artistsAndAlbumsQuery =
    "SELECT nmm:artistName(?artist) GROUP_CONCAT(nie:title(?album),'|') "
    "WHERE "
    "{ "
    "?song a nmm:MusicPiece . "
    "?song nmm:performer ?artist . "
    "?song nmm:musicAlbum ?album . "
        "} GROUP BY ?artist";

const int artistsAndAlbumsColumnCount = 2;

const QString musicQuery =
    "select ?song ?title "
    "tracker:coalesce(nmm:artistName(nmm:performer(?song)), 'UNKNOWN_ARTIST') "
    "tracker:coalesce(nie:title(nmm:musicAlbum(?song)), 'UNKNOWN_ALBUM') "
    "nfo:duration(?song) "
    "( EXISTS { ?song nao:hasTag nao:predefined-tag-favorite } ) "
    "nfo:genre(?song) nie:contentCreated(?song) "
    "tracker:id(?song) tracker:id(nmm:performer(?song)) "
    "tracker:id(nmm:musicAlbum(?song)) "
    "where { ?song a nmm:MusicPiece . "
    "?song nie:title ?title . } "
    "order by ?title "
    "<http://www.tracker-project.org/ontologies/tracker#id>(?song)";

const int musicQueryColumnCount = 11;

void readValuesFromCursor(TrackerSparqlCursor* cursor,
                          QList<QVector<QVariant> >& values)
{
    QString value;
    gint n_columns = -1;
    GError* error = 0;
    while (tracker_sparql_cursor_next (cursor, 0, &error)) {
        if (error) {
            g_error_free(error);
            return;
        }
        if (n_columns < 0)
            n_columns = tracker_sparql_cursor_get_n_columns(cursor);
        QVector<QVariant> row;
        row.reserve(n_columns);
        for (int i = 0; i < n_columns; i++) {
            // Simulating what QtSparql does with the data: get the
            // value as string, do conversion to utf-8, get the type,
            // and store the value as QList of QVector of QVariant.
            /*const TrackerSparqlValueType type =*/ tracker_sparql_cursor_get_value_type(cursor, i);
            const gchar* stringValue = tracker_sparql_cursor_get_string(cursor, i, 0);
            row.append(QVariant(QString::fromUtf8(stringValue)));
        }
        values.append(row);
    }
}

}

tst_QSparqlBenchmark::tst_QSparqlBenchmark()
{
}

tst_QSparqlBenchmark::~tst_QSparqlBenchmark()
{
}

void tst_QSparqlBenchmark::initTestCase()
{
    // For running the test without installing the plugins. Should work in
    // normal and vpath builds.
    QCoreApplication::addLibraryPath("../../../plugins");
}

void tst_QSparqlBenchmark::cleanupTestCase()
{
}

void tst_QSparqlBenchmark::init()
{
}

void tst_QSparqlBenchmark::cleanup()
{
}

namespace {
class ResultRetriever : public QObject
{
    Q_OBJECT
public:
    ResultRetriever(QSparqlResult* r)
    : result(r)
    {
        connect(r, SIGNAL(finished()),
                SLOT(onFinished()));
    }

public slots:
    void onFinished()
    {
        // Do something silly with the data
        int total = 0;
        while (result->next()) {
            total += result->value(0).toString().size();
        }
        result->deleteLater();
    }

private:
    QSparqlResult* result;

};

}

void tst_QSparqlBenchmark::queryBenchmark()
{
    QFETCH(QString, benchmarkName);
    QFETCH(QString, connectionName);
    QFETCH(QString, queryString);

    QSparqlQuery query(queryString);
    QSparqlConnection conn(connectionName);
    // Note: connection opening cost is left out of the benchmark. Connection
    // opening is async, so we need to wait here long enough to know that it has
    // opened (there is no signal sent or such to know it has opened).
    QTest::qWait(2000);

    QSparqlResult* r = 0;
    // We run multiple queries here (and don't leave it for QBENCHMARK to
    // run this multiple times, to be able to measure things like "how much
    // does adding a QThreadPool help".
    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            r = conn.exec(query);
            r->waitForFinished();
            CHECK_ERROR(r);
            QVERIFY(r->size() > 0);
            delete r;
        }
        END_BENCHMARK(benchmarkName);
    }
}

void tst_QSparqlBenchmark::queryBenchmark_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("connectionName");
    QTest::addColumn<QString>("queryString");

    QTest::newRow("TrackerDBusArtistsAndAlbums")
        << "dbus-artistsandalbums"
        << "QTRACKER"
        << artistsAndAlbumsQuery;

    QTest::newRow("TrackerDirectArtistsAndAlbums")
        << "direct-artistsandalbums"
        << "QTRACKER_DIRECT"
        << artistsAndAlbumsQuery;
}

void tst_QSparqlBenchmark::dataReadingBenchmark()
{
    QFETCH(QString, benchmarkName);
    QFETCH(QString, queryString);
    QFETCH(int, columnCount);

    // Set the dataReadyInterval to be large enough so that it won't affect this
    // test case.
    QSparqlConnectionOptions opts;
    opts.setDataReadyInterval(1000000);

    QSparqlConnection conn("QTRACKER_DIRECT", opts);
    // Note: connection opening cost is left out of the benchmark. Connection
    // opening is async, so we need to wait here long enough to know that it has
    // opened (there is no signal sent or such to know it has opened).
    QTest::qWait(2000);

    QSparqlQuery query(queryString);

    QString finished = benchmarkName + "-fin";
    QString read = benchmarkName + "-read";

    QSparqlResult* r = 0;
    for (int i = 0; i < NO_QUERIES; ++i) {
        {
            START_BENCHMARK {
                r = conn.exec(query);
                QEventLoop loop;
                connect(r, SIGNAL(finished()), &loop, SLOT(quit()));
                loop.exec();
            }
            END_BENCHMARK(finished);
        }
        CHECK_ERROR(r);
        QVERIFY(r->size() > 0);
        {
            int size = 0;
            START_BENCHMARK {
                while (r->next()) {
                    for (int c = 0; c < columnCount; ++c) {
                        QVariant var = r->value(c);
                        // Probably it's enough not to do anything with the
                        // value; the statement of getting the value won't be
                        // optimized out since calling r->value() might have
                        // side effects.
                    }
                    ++size;
                }
            }
            END_BENCHMARK(read);
            // For verifying that enough data was really read
            // qDebug() << "No of results" << size;
        }
        delete r;
    }
}

void tst_QSparqlBenchmark::dataReadingBenchmark_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("queryString");
    QTest::addColumn<int>("columnCount");

    QTest::newRow("ReadingArtistsAndAlbums")
        << "read-artistsandalbums"
        << artistsAndAlbumsQuery
        << artistsAndAlbumsColumnCount;

    QTest::newRow("ReadingMusic")
        << "read-music"
        << musicQuery
        << musicQueryColumnCount;
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparql()
{
    g_type_init();
    QFETCH(QString, benchmarkName);
    QFETCH(QString, queryString);
    GError* error = 0;
    TrackerSparqlConnection* connection = tracker_sparql_connection_get(0, &error);
    QVERIFY(connection);
    QVERIFY(error == 0);

    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            TrackerSparqlCursor* cursor =
                tracker_sparql_connection_query(connection,
                                                queryString.toUtf8(),
                                                NULL,
                                                &error);

            QVERIFY(error == 0);
            QVERIFY(cursor);

            QList<QVector<QVariant> > values;

            readValuesFromCursor(cursor, values);

            QVERIFY(values.size() > 0);

            g_object_unref(cursor);
        }
        END_BENCHMARK(benchmarkName);
    }

    g_object_unref(connection);
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparql_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("queryString");

    QTest::newRow("LibtrackerSparqlArtistsAndAlbums")
        << "lts-artistsandalbums"
        << artistsAndAlbumsQuery;

    QTest::newRow("LibtrackersparqlMusic")
        << "lts-music"
        << musicQuery;
}

namespace {
class QueryRunner : public QThread
{
public:
    QueryRunner(TrackerSparqlConnection* c, const QString& q, bool d = false)
        : connection(c), queryString(q), isDummy(d), hasRun(false)
        {
        }
    void run()
        {
            hasRun = true;
            if (isDummy)
                return;

            GError* error = 0;
            TrackerSparqlCursor* cursor =
                tracker_sparql_connection_query(connection,
                                                queryString.toUtf8(),
                                                NULL,
                                                &error);

            QVERIFY(error == 0);
            QVERIFY(cursor);

            QList<QVector<QVariant> > values;

            readValuesFromCursor(cursor, values);

            QVERIFY(values.size() > 0);
            g_object_unref(cursor);
        }
    TrackerSparqlConnection* connection;
    QString queryString;
    bool isDummy;
    bool hasRun;
};

} // unnamed namespace

void tst_QSparqlBenchmark::queryWithLibtrackerSparqlInThread()
{
    g_type_init();
    QFETCH(QString, benchmarkName);
    benchmarkName += QString("-thread");
    QFETCH(QString, queryString);
    GError* error = 0;
    TrackerSparqlConnection* connection = tracker_sparql_connection_get(0, &error);
    QVERIFY(connection);
    QVERIFY(error == 0);

    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            QueryRunner runner(connection, queryString);
            runner.start();
            runner.wait();
        }
        END_BENCHMARK(benchmarkName);
    }

    g_object_unref(connection);
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparqlInThread_data()
{
    queryWithLibtrackerSparql_data();
}

void tst_QSparqlBenchmark::threadCreatingOverhead()
{
    QBENCHMARK {
        for (int i = 0; i < NO_QUERIES; ++i) {
            QueryRunner runner(0, "", true);
            runner.start();
            runner.wait();
            QVERIFY(runner.hasRun);
        }
    }
}

QTEST_MAIN(tst_QSparqlBenchmark)
#include "tst_qsparql_benchmark.moc"
<commit_msg>Print median and arithmetic mean for each benchmark<commit_after>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (ivan.frade@nokia.com)
**
** This file is part of the test suite of the QtSparql module (not yet part of the Qt Toolkit).
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at ivan.frade@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "../testhelpers.h"

#include <tracker-sparql.h>

#include <QtTest/QtTest>
#include <QtSparql/QtSparql>

#include <sys/time.h>
#include <stdio.h>

#define START_BENCHMARK \
    int benchmarkTotal=0; { \
    struct timeval benchmark_tv; \
    gettimeofday(&benchmark_tv, NULL); \
    const long benchmark_start = benchmark_tv.tv_sec * 1000 + (benchmark_tv.tv_usec + 500) / 1000; \
    do

#define END_BENCHMARK(BENCHMARKNAME) while(false); \
        gettimeofday(&benchmark_tv, NULL); \
        const long benchmark_end = benchmark_tv.tv_sec * 1000 + (benchmark_tv.tv_usec + 500) / 1000; \
        char benchmark_tsbuf[80]; \
        qsnprintf(benchmark_tsbuf, sizeof(benchmark_tsbuf), "QSparql_benchmark_data: %s %lu\n",  \
                  qPrintable(BENCHMARKNAME), benchmark_end - benchmark_start); \
        fwrite(benchmark_tsbuf, qstrnlen(benchmark_tsbuf, sizeof(benchmark_tsbuf)), 1, stderr); \
        benchmarkTotal += benchmark_end - benchmark_start; \
    }

#define PRINT_STATS(BENCHMARKNAME, LIST) { \
    qSort(LIST); \
    int median = (LIST.size()+1)/2; \
    int average = 0; \
    for(int i=0;i<LIST.size();i++) \
        average += LIST[i]; \
    fprintf(stderr, "%s median value = %i\n", qPrintable(BENCHMARKNAME), LIST[median]); \
    fprintf(stderr, "%s mean value = %i\n", qPrintable(BENCHMARKNAME), average/LIST.size()); }

#define NO_QUERIES 100

class tst_QSparqlBenchmark : public QObject
{
    Q_OBJECT

public:
    tst_QSparqlBenchmark();
    virtual ~tst_QSparqlBenchmark();

public slots:
    void initTestCase();
    void cleanupTestCase();
    void init();
    void cleanup();

private slots:
    // Actual benchmarks
    void queryBenchmark();
    void queryBenchmark_data();

    void dataReadingBenchmark();
    void dataReadingBenchmark_data();

    // Reference benchmarks
    void queryWithLibtrackerSparql();
    void queryWithLibtrackerSparql_data();

    void queryWithLibtrackerSparqlInThread();
    void queryWithLibtrackerSparqlInThread_data();

    void threadCreatingOverhead();
};

namespace {
    // Query strings

    // Test data for running this can be found in the tracker project.
const QString artistsAndAlbumsQuery =
    "SELECT nmm:artistName(?artist) GROUP_CONCAT(nie:title(?album),'|') "
    "WHERE "
    "{ "
    "?song a nmm:MusicPiece . "
    "?song nmm:performer ?artist . "
    "?song nmm:musicAlbum ?album . "
        "} GROUP BY ?artist";

const int artistsAndAlbumsColumnCount = 2;

const QString musicQuery =
    "select ?song ?title "
    "tracker:coalesce(nmm:artistName(nmm:performer(?song)), 'UNKNOWN_ARTIST') "
    "tracker:coalesce(nie:title(nmm:musicAlbum(?song)), 'UNKNOWN_ALBUM') "
    "nfo:duration(?song) "
    "( EXISTS { ?song nao:hasTag nao:predefined-tag-favorite } ) "
    "nfo:genre(?song) nie:contentCreated(?song) "
    "tracker:id(?song) tracker:id(nmm:performer(?song)) "
    "tracker:id(nmm:musicAlbum(?song)) "
    "where { ?song a nmm:MusicPiece . "
    "?song nie:title ?title . } "
    "order by ?title "
    "<http://www.tracker-project.org/ontologies/tracker#id>(?song)";

const int musicQueryColumnCount = 11;

void readValuesFromCursor(TrackerSparqlCursor* cursor,
                          QList<QVector<QVariant> >& values)
{
    QString value;
    gint n_columns = -1;
    GError* error = 0;
    while (tracker_sparql_cursor_next (cursor, 0, &error)) {
        if (error) {
            g_error_free(error);
            return;
        }
        if (n_columns < 0)
            n_columns = tracker_sparql_cursor_get_n_columns(cursor);
        QVector<QVariant> row;
        row.reserve(n_columns);
        for (int i = 0; i < n_columns; i++) {
            // Simulating what QtSparql does with the data: get the
            // value as string, do conversion to utf-8, get the type,
            // and store the value as QList of QVector of QVariant.
            /*const TrackerSparqlValueType type =*/ tracker_sparql_cursor_get_value_type(cursor, i);
            const gchar* stringValue = tracker_sparql_cursor_get_string(cursor, i, 0);
            row.append(QVariant(QString::fromUtf8(stringValue)));
        }
        values.append(row);
    }
}

}

tst_QSparqlBenchmark::tst_QSparqlBenchmark()
{
}

tst_QSparqlBenchmark::~tst_QSparqlBenchmark()
{
}

void tst_QSparqlBenchmark::initTestCase()
{
    // For running the test without installing the plugins. Should work in
    // normal and vpath builds.
    QCoreApplication::addLibraryPath("../../../plugins");
}

void tst_QSparqlBenchmark::cleanupTestCase()
{
}

void tst_QSparqlBenchmark::init()
{
}

void tst_QSparqlBenchmark::cleanup()
{
}

namespace {
class ResultRetriever : public QObject
{
    Q_OBJECT
public:
    ResultRetriever(QSparqlResult* r)
    : result(r)
    {
        connect(r, SIGNAL(finished()),
                SLOT(onFinished()));
    }

public slots:
    void onFinished()
    {
        // Do something silly with the data
        int total = 0;
        while (result->next()) {
            total += result->value(0).toString().size();
        }
        result->deleteLater();
    }

private:
    QSparqlResult* result;

};

}

void tst_QSparqlBenchmark::queryBenchmark()
{
    QFETCH(QString, benchmarkName);
    QFETCH(QString, connectionName);
    QFETCH(QString, queryString);

    QSparqlQuery query(queryString);
    QSparqlConnection conn(connectionName);
    // Note: connection opening cost is left out of the benchmark. Connection
    // opening is async, so we need to wait here long enough to know that it has
    // opened (there is no signal sent or such to know it has opened).
    QTest::qWait(2000);

    QSparqlResult* r = 0;
    QList<int> totalTimes;
    // We run multiple queries here (and don't leave it for QBENCHMARK to
    // run this multiple times, to be able to measure things like "how much
    // does adding a QThreadPool help".
    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            r = conn.exec(query);
            r->waitForFinished();
            CHECK_ERROR(r);
            QVERIFY(r->size() > 0);
            delete r;
        }
        END_BENCHMARK(benchmarkName);
        totalTimes.append(benchmarkTotal);
    }
    PRINT_STATS(benchmarkName, totalTimes);
}

void tst_QSparqlBenchmark::queryBenchmark_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("connectionName");
    QTest::addColumn<QString>("queryString");

    QTest::newRow("TrackerDBusArtistsAndAlbums")
        << "dbus-artistsandalbums"
        << "QTRACKER"
        << artistsAndAlbumsQuery;

    QTest::newRow("TrackerDirectArtistsAndAlbums")
        << "direct-artistsandalbums"
        << "QTRACKER_DIRECT"
        << artistsAndAlbumsQuery;
}

void tst_QSparqlBenchmark::dataReadingBenchmark()
{
    QFETCH(QString, benchmarkName);
    QFETCH(QString, queryString);
    QFETCH(int, columnCount);

    // Set the dataReadyInterval to be large enough so that it won't affect this
    // test case.
    QSparqlConnectionOptions opts;
    opts.setDataReadyInterval(1000000);

    QSparqlConnection conn("QTRACKER_DIRECT", opts);
    // Note: connection opening cost is left out of the benchmark. Connection
    // opening is async, so we need to wait here long enough to know that it has
    // opened (there is no signal sent or such to know it has opened).
    QTest::qWait(2000);

    QSparqlQuery query(queryString);

    QString finished = benchmarkName + "-fin";
    QString read = benchmarkName + "-read";

    QSparqlResult* r = 0;
    QList<int> totalTimesFinished;
    QList<int> totalTimesRead;

    for (int i = 0; i < NO_QUERIES; ++i) {
        {
            START_BENCHMARK {
                r = conn.exec(query);
                QEventLoop loop;
                connect(r, SIGNAL(finished()), &loop, SLOT(quit()));
                loop.exec();
            }
            END_BENCHMARK(finished);
            totalTimesFinished.append(benchmarkTotal);
        }
        CHECK_ERROR(r);
        QVERIFY(r->size() > 0);
        {
            int size = 0;
            START_BENCHMARK {
                while (r->next()) {
                    for (int c = 0; c < columnCount; ++c) {
                        QVariant var = r->value(c);
                        // Probably it's enough not to do anything with the
                        // value; the statement of getting the value won't be
                        // optimized out since calling r->value() might have
                        // side effects.
                    }
                    ++size;
                }
            }
            END_BENCHMARK(read);
            totalTimesRead.append(benchmarkTotal);
            // For verifying that enough data was really read
            // qDebug() << "No of results" << size;
        }
        delete r;
    }
    PRINT_STATS(finished, totalTimesFinished);
    PRINT_STATS(read, totalTimesRead);
}

void tst_QSparqlBenchmark::dataReadingBenchmark_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("queryString");
    QTest::addColumn<int>("columnCount");

    QTest::newRow("ReadingArtistsAndAlbums")
        << "read-artistsandalbums"
        << artistsAndAlbumsQuery
        << artistsAndAlbumsColumnCount;

    QTest::newRow("ReadingMusic")
        << "read-music"
        << musicQuery
        << musicQueryColumnCount;
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparql()
{
    g_type_init();
    QFETCH(QString, benchmarkName);
    QFETCH(QString, queryString);
    GError* error = 0;
    TrackerSparqlConnection* connection = tracker_sparql_connection_get(0, &error);
    QVERIFY(connection);
    QVERIFY(error == 0);
    QList<int> totalTimes;
    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            TrackerSparqlCursor* cursor =
                tracker_sparql_connection_query(connection,
                                                queryString.toUtf8(),
                                                NULL,
                                                &error);

            QVERIFY(error == 0);
            QVERIFY(cursor);

            QList<QVector<QVariant> > values;

            readValuesFromCursor(cursor, values);

            QVERIFY(values.size() > 0);

            g_object_unref(cursor);
        }
        END_BENCHMARK(benchmarkName);
        totalTimes.append(benchmarkTotal);
    }

    g_object_unref(connection);
    PRINT_STATS(benchmarkName,totalTimes);
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparql_data()
{
    QTest::addColumn<QString>("benchmarkName");
    QTest::addColumn<QString>("queryString");

    QTest::newRow("LibtrackerSparqlArtistsAndAlbums")
        << "lts-artistsandalbums"
        << artistsAndAlbumsQuery;

    QTest::newRow("LibtrackersparqlMusic")
        << "lts-music"
        << musicQuery;
}

namespace {
class QueryRunner : public QThread
{
public:
    QueryRunner(TrackerSparqlConnection* c, const QString& q, bool d = false)
        : connection(c), queryString(q), isDummy(d), hasRun(false)
        {
        }
    void run()
        {
            hasRun = true;
            if (isDummy)
                return;

            GError* error = 0;
            TrackerSparqlCursor* cursor =
                tracker_sparql_connection_query(connection,
                                                queryString.toUtf8(),
                                                NULL,
                                                &error);

            QVERIFY(error == 0);
            QVERIFY(cursor);

            QList<QVector<QVariant> > values;

            readValuesFromCursor(cursor, values);

            QVERIFY(values.size() > 0);
            g_object_unref(cursor);
        }
    TrackerSparqlConnection* connection;
    QString queryString;
    bool isDummy;
    bool hasRun;
};

} // unnamed namespace

void tst_QSparqlBenchmark::queryWithLibtrackerSparqlInThread()
{
    g_type_init();
    QFETCH(QString, benchmarkName);
    benchmarkName += QString("-thread");
    QFETCH(QString, queryString);
    GError* error = 0;
    TrackerSparqlConnection* connection = tracker_sparql_connection_get(0, &error);
    QVERIFY(connection);
    QVERIFY(error == 0);
    QList<int> totalTimes;
    for (int i = 0; i < NO_QUERIES; ++i) {
        START_BENCHMARK {
            QueryRunner runner(connection, queryString);
            runner.start();
            runner.wait();
        }
        END_BENCHMARK(benchmarkName);
        totalTimes.append(benchmarkTotal);
    }

    g_object_unref(connection);
    PRINT_STATS(benchmarkName, totalTimes);
}

void tst_QSparqlBenchmark::queryWithLibtrackerSparqlInThread_data()
{
    queryWithLibtrackerSparql_data();
}

void tst_QSparqlBenchmark::threadCreatingOverhead()
{
    QBENCHMARK {
        for (int i = 0; i < NO_QUERIES; ++i) {
            QueryRunner runner(0, "", true);
            runner.start();
            runner.wait();
            QVERIFY(runner.hasRun);
        }
    }
}

QTEST_MAIN(tst_QSparqlBenchmark)
#include "tst_qsparql_benchmark.moc"
<|endoftext|>


<commit_before>#ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_

#include <boost/random.hpp>

#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>

template <typename PointInT, typename StateT> bool
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initCompute ()
{
  if (!Tracker<PointInT, StateT>::initCompute ())
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  coherence_->setTargetCloud (input_);

  if (!particles_ || particles_->points.empty ())
    initParticles ();
  return (true);
}

template <typename PointInT, typename StateT> double
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::calcLikelihood (const StateT &hypothesis)
{
  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
  PointCloudInPtr transed_reference = PointCloudInPtr (new PointCloudIn ());
  pcl::transformPointCloudWithNormals<PointInT> (*ref_, *transed_reference, trans);

  // search the nearest pairs
  std::vector<int> k_indices(1);
  std::vector<float> k_distances(1);
  int num_points = 0;
  IndicesPtr indices = IndicesPtr (new std::vector<int> ());
  for ( size_t i = 0; i < transed_reference->points.size (); i++ )
  {
    PointInT input_point = transed_reference->points[i];

    if (!pcl_isfinite(input_point.x) || !pcl_isfinite(input_point.y) || !pcl_isfinite(input_point.z))
      continue;
    // take occlusion into account
    Eigen::Vector4f p = input_point.getVector4fMap ();
    Eigen::Vector4f n = input_point.getNormalVector4fMap ();
    double theta = pcl::getAngle3D(p, n);

    // TODO: check NAN
    if ( theta > occlusion_angle_thr_ )
    {
      indices->push_back (i);
      ++num_points;
    }
  }

  //coherence_->setInputCloud (transed_reference);
  //coherence_->setIndices (indices);
  double val = coherence_->compute (transed_reference, indices);

  // take min_indices_ into account
  // TODO: normalization of likelihood is required?
  double likelihood;
  if ( num_points >= min_indices_ )
    likelihood = val;
  //likelihood = exp((((ret * ret ) / num) / (- 2.0 * sigma2)));
  else
    likelihood = 0.0;
  return likelihood;
}

template <typename PointInT, typename StateT> int
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement
(const std::vector<int>& a, const std::vector<double>& q)
{
  using namespace boost;
  static mt19937 gen (static_cast<unsigned long>(time (0)));
  uniform_real<> dst (0.0, 1.0);
  variate_generator<mt19937&, uniform_real<> > rand (gen, dst);
  double rU = rand () * particle_num_;
  int k = (int)rU;
  rU -= k;    /* rU - [rU] */
  if ( rU < q[k] )
    return k;
  else
    return a[k];
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::genAliasTable (std::vector<int> &a, std::vector<double> &q)
{
  /* generate an alias table, a and q */
  std::vector<int> HL (particle_num_);
  std::vector<int>::iterator H = HL.begin ();
  std::vector<int>::iterator L = HL.end () - 1;
  for ( int i = 0; i < particle_num_; i++ )
    q[i] = particles_->points[i].weight * particle_num_;
  for ( int i = 0; i < particle_num_; i++ )
    a[i] = i;
  // setup H and L
  for ( int i = 0; i < particle_num_; i++ )
    if ( q[i] >= 1.0 )
      *H++ = i;
    else
      *L-- = i;

  while ( H != HL.begin() && L != HL.end() - 1 )
  {
    int j = *(L + 1);
    int k = *(H - 1);
    a[j] = k;
    q[k] += q[j] - 1;
    L++;
    if ( q[k] < 1.0 )
    {
      *L-- = k;
      --H;
    }
  }
}

template <typename PointInT, typename StateT> Eigen::Affine3f
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::toEigenMatrix (const StateT& particle)
{
  return particle.toEigenMatrix ();
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initParticles ()
{
  PCL_INFO ("[pcl::%s::initParticles] initializing...\n", getClassName ().c_str ());
  particles_.reset( new PointCloudState ());

  representative_state_.zero ();
  representative_state_.weight = 0.0;

  std::vector<double> initial_noise_mean;
  StateT offset = StateT::toState (trans_);
  representative_state_ = offset;
  // sampling...
  for ( int i = 0; i < particle_num_; i++ )
  {
    StateT p;
    p.weight = 1.0 / particle_num_;
    p.zero ();
    p.sample (initial_noise_mean_, initial_noise_covariance_);
    p = p + offset;
    particles_->points.push_back (p); // update
  }
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::normalizeWeight ()
{
  double sum = 0.0;
  for ( int i = 0; i < particle_num_; i++ )
    sum += particles_->points[i].weight;
  for ( int i = 0; i < particle_num_; i++ )
    particles_->points[i].weight =  particles_->points[i].weight / sum;
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::weight ()
{
  coherence_->initCompute ();
  for ( int i = 0; i < particle_num_; i++ )
  {
    double likelihood = calcLikelihood (particles_->points[i]);
    particles_->points[i].weight = likelihood;
  }
  normalizeWeight ();
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resample ()
{
  std::vector<int> a (particle_num_);
  std::vector<double> q (particle_num_);
  genAliasTable (a, q);

  const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
  // memoize the original list of particles
  //std::vector<Particle> origparticles = particles_;
  PointCloudStatePtr  origparticles = particles_;
  particles_->points.clear ();
  // the first particle, it is a just copy of the maximum result
  pcl::tracking::ParticleXYZRPY p = representative_state_;
  particles_->points.push_back (p);
  for ( int i = 1; i < particle_num_; i++ )
  {
    int target_particle_index = sampleWithReplacement (a, q);
    pcl::tracking::ParticleXYZRPY p = origparticles->points[target_particle_index];
    // add noise using gaussian
    p.sample (zero_mean, step_noise_covariance_);
    particles_->points.push_back (p);
  }
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::update ()
{
  representative_state_.zero ();

  double maxweight = particles_->points[0].weight;
  int maxindex = 0;
  for ( int i = 1; i < particle_num_; i++)
  {
    StateT p = particles_->points[i];
    if (maxweight < p.weight )
    {
      maxweight = p.weight;
      maxindex = i;
    }
  }
  representative_state_ = particles_->points[maxindex];
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTracking ()
{

  for (int i = 0; i < iteration_num_; i++)
  {
    resample ();
    weight (); // likelihood is called in it
    update ();
  }

  if ( getResult ().weight < resample_likelihood_thr_ )
  {
    PCL_WARN ("too small likelihood, re-initializing...\n");
    initParticles ();
  }
}

#endif
<commit_msg>using the template arguments<commit_after>#ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_

#include <boost/random.hpp>

#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>

template <typename PointInT, typename StateT> bool
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initCompute ()
{
  if (!Tracker<PointInT, StateT>::initCompute ())
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  coherence_->setTargetCloud (input_);

  if (!particles_ || particles_->points.empty ())
    initParticles ();
  return (true);
}

template <typename PointInT, typename StateT> double
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::calcLikelihood (const StateT &hypothesis)
{
  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
  PointCloudInPtr transed_reference = PointCloudInPtr (new PointCloudIn ());
  pcl::transformPointCloudWithNormals<PointInT> (*ref_, *transed_reference, trans);

  // search the nearest pairs
  std::vector<int> k_indices(1);
  std::vector<float> k_distances(1);
  int num_points = 0;
  IndicesPtr indices = IndicesPtr (new std::vector<int> ());
  for ( size_t i = 0; i < transed_reference->points.size (); i++ )
  {
    PointInT input_point = transed_reference->points[i];

    if (!pcl_isfinite(input_point.x) || !pcl_isfinite(input_point.y) || !pcl_isfinite(input_point.z))
      continue;
    // take occlusion into account
    Eigen::Vector4f p = input_point.getVector4fMap ();
    Eigen::Vector4f n = input_point.getNormalVector4fMap ();
    double theta = pcl::getAngle3D(p, n);

    // TODO: check NAN
    if ( theta > occlusion_angle_thr_ )
    {
      indices->push_back (i);
      ++num_points;
    }
  }

  //coherence_->setInputCloud (transed_reference);
  //coherence_->setIndices (indices);
  double val = coherence_->compute (transed_reference, indices);

  // take min_indices_ into account
  // TODO: normalization of likelihood is required?
  double likelihood;
  if ( num_points >= min_indices_ )
    likelihood = val / num_points;
  //likelihood = exp((((ret * ret ) / num) / (- 2.0 * sigma2)));
  else
    likelihood = 0.0;
  return likelihood;
}

template <typename PointInT, typename StateT> int
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement
(const std::vector<int>& a, const std::vector<double>& q)
{
  using namespace boost;
  static mt19937 gen (static_cast<unsigned long>(time (0)));
  uniform_real<> dst (0.0, 1.0);
  variate_generator<mt19937&, uniform_real<> > rand (gen, dst);
  double rU = rand () * particle_num_;
  int k = (int)rU;
  rU -= k;    /* rU - [rU] */
  if ( rU < q[k] )
    return k;
  else
    return a[k];
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::genAliasTable (std::vector<int> &a, std::vector<double> &q)
{
  /* generate an alias table, a and q */
  std::vector<int> HL (particle_num_);
  std::vector<int>::iterator H = HL.begin ();
  std::vector<int>::iterator L = HL.end () - 1;
  for ( int i = 0; i < particle_num_; i++ )
    q[i] = particles_->points[i].weight * particle_num_;
  for ( int i = 0; i < particle_num_; i++ )
    a[i] = i;
  // setup H and L
  for ( int i = 0; i < particle_num_; i++ )
    if ( q[i] >= 1.0 )
      *H++ = i;
    else
      *L-- = i;

  while ( H != HL.begin() && L != HL.end() - 1 )
  {
    int j = *(L + 1);
    int k = *(H - 1);
    a[j] = k;
    q[k] += q[j] - 1;
    L++;
    if ( q[k] < 1.0 )
    {
      *L-- = k;
      --H;
    }
  }
}

template <typename PointInT, typename StateT> Eigen::Affine3f
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::toEigenMatrix (const StateT& particle)
{
  return particle.toEigenMatrix ();
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initParticles ()
{
  PCL_INFO ("[pcl::%s::initParticles] initializing...\n", getClassName ().c_str ());
  particles_.reset (new PointCloudState ());

  representative_state_.zero ();
  representative_state_.weight = 0.0;

  std::vector<double> initial_noise_mean;
  StateT offset = StateT::toState (trans_);
  representative_state_ = offset;
  // sampling...
  for ( int i = 0; i < particle_num_; i++ )
  {
    StateT p;
    p.zero ();
    p.weight = 1.0 / particle_num_;
    p.sample (initial_noise_mean_, initial_noise_covariance_);
    p = p + offset;
    particles_->points.push_back (p); // update
  }
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::normalizeWeight ()
{
  double sum = 0.0;
  for ( int i = 0; i < particle_num_; i++ )
    sum += particles_->points[i].weight;
  for ( int i = 0; i < particle_num_; i++ )
    particles_->points[i].weight =  particles_->points[i].weight / sum;
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::weight ()
{
  coherence_->initCompute ();
  for ( int i = 0; i < particle_num_; i++ )
  {
    double likelihood = calcLikelihood (particles_->points[i]);
    particles_->points[i].weight = likelihood;
  }
  normalizeWeight ();
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resample ()
{
  std::vector<int> a (particle_num_);
  std::vector<double> q (particle_num_);
  genAliasTable (a, q);

  const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
  // memoize the original list of particles
  //std::vector<Particle> origparticles = particles_;
  PointCloudStatePtr origparticles = particles_;
  particles_->points.clear ();
  // the first particle, it is a just copy of the maximum result
  StateT p = representative_state_;
  particles_->points.push_back (p);
  for ( int i = 1; i < particle_num_; i++ )
  {
    int target_particle_index = sampleWithReplacement (a, q);
    StateT p = origparticles->points[target_particle_index];
    // add noise using gaussian
    p.sample (zero_mean, step_noise_covariance_);
    particles_->points.push_back (p);
  }
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::update ()
{
  representative_state_.zero ();

  double maxweight = particles_->points[0].weight;
  int maxindex = 0;
  for ( int i = 1; i < particle_num_; i++)
  {
    StateT p = particles_->points[i];
    if (maxweight < p.weight )
    {
      maxweight = p.weight;
      maxindex = i;
    }
  }
  //if (representative_state_.weight < maxweight)
  representative_state_ = particles_->points[maxindex];
}

template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTracking ()
{

  for (int i = 0; i < iteration_num_; i++)
  {
    resample ();
    weight (); // likelihood is called in it
    update ();
  }

  if ( getResult ().weight < resample_likelihood_thr_ )
  {
    PCL_WARN ("too small likelihood, re-initializing...\n");
    initParticles ();
  }
}

#endif
<|endoftext|>


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

/* Based on
 *
       SUBROUTINE DGETF2( M, N, A, LDA, IPIV, INFO )
 *
 *  -- LAPACK routine (version 3.2) --
 *  -- LAPACK is a software package provided by Univ. of Tennessee,    --
 *  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
 *     November 2006
 */

#ifndef FLENS_LAPACK_GESV_TF2_TCC
#define FLENS_LAPACK_GESV_TF2_TCC 1

#include <algorithm>
#include <flens/blas/blas.h>
#include <flens/lapack/lapack.h>

namespace flens { namespace lapack {

//== generic lapack implementation =============================================

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2_generic(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    using lapack::lamch;
    using std::abs;
    using std::min;

    typedef typename GeMatrix<MA>::IndexType    IndexType;
    typedef typename GeMatrix<MA>::ElementType  T;

    const Underscore<IndexType> _;

    const T  Zero(0), One(1);

    const IndexType m = A.numRows();
    const IndexType n = A.numCols();

    IndexType info = 0;

//
//  Quick return if possible
//
    if (m==0 || n==0) {
        return info;
    }
//
//     Compute machine safe minimum
//
    auto safeMin = lamch<T>(SafeMin);

    for (IndexType j=1; j<=min(m,n); ++j) {
//
//      Row range of current submatrix A(j:M, j:N)
//
        const auto rows = _(j, m);
//
//      Row and column range of trailing submatrix A(j+1:M, j+1:N)
//
        const auto _rows = _(j+1, m);
        const auto _cols = _(j+1, n);
//
//      Find pivot and test for singularity.
//
        IndexType jp = j - 1 + blas::iamax(A(rows,j));
        piv(j) = jp;
        if (A(jp,j)!=Zero) {
//
//          Apply the interchange to columns 1:N.
//
            if (j!=jp) {
                blas::swap(A(j,_), A(jp,_));
            }
//
//          Compute elements J+1:M of J-th column.
//
            if (j<m) {
                if (abs(A(j,j))>=safeMin) {
                    blas::scal(One/A(j, j), A(_rows,j));
                } else {
                    for (IndexType i=1; i<=m-j; ++i) {
                        A(j+i,j) /= A(j,j);
                    }
                }
            }
        } else {
            if (info==0) {
                info = j;
            }
        }
//
//      Update trailing submatrix A(j+1:M, j+1:N)
//
        if (j<min(m,n)) {
            blas::ru(-One, A(_rows,j), A(j,_cols), A(_rows,_cols));
        }
    }

    return info;
}

//== interface for native lapack ===============================================

#ifdef CHECK_CXXLAPACK

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2_native(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    return cxxlapack::getf2<INTEGER>(A.numRows(), A.numCols(),
                                     A.data(), A.leadingDimension(),
                                     piv.data());
}

#endif // CHECK_CXXLAPACK

//== public interface ==========================================================

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    typedef typename GeMatrix<MA>::IndexType    IndexType;

//
//  Test the input parameters
//
    ASSERT(A.firstRow()==1);
    ASSERT(A.firstCol()==1);
    ASSERT((piv.inc()>0 && piv.firstIndex()==1)
        || (piv.inc()<0 && piv.firstIndex()==A.numRows()));

#   ifdef CHECK_CXXLAPACK
//
//  Make copies of output arguments
//
    typename GeMatrix<MA>::NoView       A_org      = A;
    typename DenseVector<VP>::NoView    piv_org    = piv;
#   endif

//
//  Call implementation
//
    std::cerr << "enter" << std::endl;
    IndexType info = tf2_generic(A, piv);
    std::cerr << "leave" << std::endl;

#   ifdef CHECK_CXXLAPACK
//
//  Make copies of results
//
    typename GeMatrix<MA>::NoView       A_generic      = A;
    typename DenseVector<VP>::NoView    piv_generic    = piv;
//
//  Restore output arguments
//
    A   = A_org;
    piv = piv_org;
//
//  Compare results
//
    IndexType _info = tf2_native(A, piv);

    bool failed = false;
    if (! isIdentical(A_generic, A, "A_generic", "A")) {
        std::cerr << "CXXLAPACK: A_generic = " << A_generic << std::endl;
        std::cerr << "F77LAPACK: A = " << A << std::endl;
        failed = true;
    }

    if (! isIdentical(piv_generic, piv, "piv_generic", "piv")) {
        std::cerr << "CXXLAPACK: piv_generic = " << piv_generic << std::endl;
        std::cerr << "F77LAPACK: piv = " << piv << std::endl;
        failed = true;
    }

    if (! isIdentical(info, _info, " info", "_info")) {
        std::cerr << "CXXLAPACK:  info = " << info << std::endl;
        std::cerr << "F77LAPACK: _info = " << _info << std::endl;
        failed = true;
    }

    if (failed) {
        std::cerr << "A_org = " << A_org << std::endl;
        ASSERT(0);
    }
#   endif

    return info;
}

//-- forwarding ----------------------------------------------------------------
template <typename MA, typename VP>
typename MA::IndexType
tf2(MA &&A, VP &&piv)
{
    typedef typename MA::IndexType  IndexType;

    CHECKPOINT_ENTER;
    IndexType info =  tf2(A, piv);
    CHECKPOINT_LEAVE;

    return info;
}

} } // namespace lapack, flens

#endif // FLENS_LAPACK_GESV_TF2_TCC
<commit_msg>Removed a debug-output ...<commit_after>/*
 *   Copyright (c) 2011, Michael Lehn
 *
 *   All rights reserved.
 *
 *   Redistribution and use in source and binary forms, with or without
 *   modification, are permitted provided that the following conditions
 *   are met:
 *
 *   1) Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *   2) Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in
 *      the documentation and/or other materials provided with the
 *      distribution.
 *   3) Neither the name of the FLENS development group nor the names of
 *      its contributors may be used to endorse or promote products derived
 *      from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/* Based on
 *
       SUBROUTINE DGETF2( M, N, A, LDA, IPIV, INFO )
 *
 *  -- LAPACK routine (version 3.2) --
 *  -- LAPACK is a software package provided by Univ. of Tennessee,    --
 *  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
 *     November 2006
 */

#ifndef FLENS_LAPACK_GESV_TF2_TCC
#define FLENS_LAPACK_GESV_TF2_TCC 1

#include <algorithm>
#include <flens/blas/blas.h>
#include <flens/lapack/lapack.h>

namespace flens { namespace lapack {

//== generic lapack implementation =============================================

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2_generic(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    using lapack::lamch;
    using std::abs;
    using std::min;

    typedef typename GeMatrix<MA>::IndexType    IndexType;
    typedef typename GeMatrix<MA>::ElementType  T;

    const Underscore<IndexType> _;

    const T  Zero(0), One(1);

    const IndexType m = A.numRows();
    const IndexType n = A.numCols();

    IndexType info = 0;

//
//  Quick return if possible
//
    if (m==0 || n==0) {
        return info;
    }
//
//     Compute machine safe minimum
//
    auto safeMin = lamch<T>(SafeMin);

    for (IndexType j=1; j<=min(m,n); ++j) {
//
//      Row range of current submatrix A(j:M, j:N)
//
        const auto rows = _(j, m);
//
//      Row and column range of trailing submatrix A(j+1:M, j+1:N)
//
        const auto _rows = _(j+1, m);
        const auto _cols = _(j+1, n);
//
//      Find pivot and test for singularity.
//
        IndexType jp = j - 1 + blas::iamax(A(rows,j));
        piv(j) = jp;
        if (A(jp,j)!=Zero) {
//
//          Apply the interchange to columns 1:N.
//
            if (j!=jp) {
                blas::swap(A(j,_), A(jp,_));
            }
//
//          Compute elements J+1:M of J-th column.
//
            if (j<m) {
                if (abs(A(j,j))>=safeMin) {
                    blas::scal(One/A(j, j), A(_rows,j));
                } else {
                    for (IndexType i=1; i<=m-j; ++i) {
                        A(j+i,j) /= A(j,j);
                    }
                }
            }
        } else {
            if (info==0) {
                info = j;
            }
        }
//
//      Update trailing submatrix A(j+1:M, j+1:N)
//
        if (j<min(m,n)) {
            blas::ru(-One, A(_rows,j), A(j,_cols), A(_rows,_cols));
        }
    }

    return info;
}

//== interface for native lapack ===============================================

#ifdef CHECK_CXXLAPACK

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2_native(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    return cxxlapack::getf2<INTEGER>(A.numRows(), A.numCols(),
                                     A.data(), A.leadingDimension(),
                                     piv.data());
}

#endif // CHECK_CXXLAPACK

//== public interface ==========================================================

template <typename MA, typename VP>
typename GeMatrix<MA>::IndexType
tf2(GeMatrix<MA> &A, DenseVector<VP> &piv)
{
    typedef typename GeMatrix<MA>::IndexType    IndexType;

//
//  Test the input parameters
//
    ASSERT(A.firstRow()==1);
    ASSERT(A.firstCol()==1);
    ASSERT((piv.inc()>0 && piv.firstIndex()==1)
        || (piv.inc()<0 && piv.firstIndex()==A.numRows()));

#   ifdef CHECK_CXXLAPACK
//
//  Make copies of output arguments
//
    typename GeMatrix<MA>::NoView       A_org      = A;
    typename DenseVector<VP>::NoView    piv_org    = piv;
#   endif

//
//  Call implementation
//
    IndexType info = tf2_generic(A, piv);

#   ifdef CHECK_CXXLAPACK
//
//  Make copies of results
//
    typename GeMatrix<MA>::NoView       A_generic      = A;
    typename DenseVector<VP>::NoView    piv_generic    = piv;
//
//  Restore output arguments
//
    A   = A_org;
    piv = piv_org;
//
//  Compare results
//
    IndexType _info = tf2_native(A, piv);

    bool failed = false;
    if (! isIdentical(A_generic, A, "A_generic", "A")) {
        std::cerr << "CXXLAPACK: A_generic = " << A_generic << std::endl;
        std::cerr << "F77LAPACK: A = " << A << std::endl;
        failed = true;
    }

    if (! isIdentical(piv_generic, piv, "piv_generic", "piv")) {
        std::cerr << "CXXLAPACK: piv_generic = " << piv_generic << std::endl;
        std::cerr << "F77LAPACK: piv = " << piv << std::endl;
        failed = true;
    }

    if (! isIdentical(info, _info, " info", "_info")) {
        std::cerr << "CXXLAPACK:  info = " << info << std::endl;
        std::cerr << "F77LAPACK: _info = " << _info << std::endl;
        failed = true;
    }

    if (failed) {
        std::cerr << "A_org = " << A_org << std::endl;
        ASSERT(0);
    }
#   endif

    return info;
}

//-- forwarding ----------------------------------------------------------------
template <typename MA, typename VP>
typename MA::IndexType
tf2(MA &&A, VP &&piv)
{
    typedef typename MA::IndexType  IndexType;

    CHECKPOINT_ENTER;
    IndexType info =  tf2(A, piv);
    CHECKPOINT_LEAVE;

    return info;
}

} } // namespace lapack, flens

#endif // FLENS_LAPACK_GESV_TF2_TCC
<|endoftext|>