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

#include <shogun/evaluation/CrossValidation.h>
#include <shogun/machine/Machine.h>
#include <shogun/evaluation/Evaluation.h>
#include <shogun/evaluation/SplittingStrategy.h>
#include <shogun/base/Parameter.h>
#include <shogun/mathematics/Statistics.h>
#include <shogun/evaluation/CrossValidationOutput.h>
#include <shogun/lib/List.h>

using namespace shogun;

CCrossValidation::CCrossValidation() : CMachineEvaluation()
{
	init();
}

CCrossValidation::CCrossValidation(CMachine* machine, CFeatures* features,
		CLabels* labels, CSplittingStrategy* splitting_strategy,
		CEvaluation* evaluation_criterion, bool autolock) :
		CMachineEvaluation(machine, features, labels, splitting_strategy,
		evaluation_criterion, autolock)
{
	init();
}

CCrossValidation::CCrossValidation(CMachine* machine, CLabels* labels,
		CSplittingStrategy* splitting_strategy,
		CEvaluation* evaluation_criterion, bool autolock) :
		CMachineEvaluation(machine, labels, splitting_strategy, evaluation_criterion,
		autolock)
{
	init();
}

CCrossValidation::~CCrossValidation()
{
	SG_UNREF(m_xval_outputs);
}

void CCrossValidation::init()
{
	m_num_runs=1;

	/* do reference counting for output objects */
	m_xval_outputs=new CList(true);

	SG_ADD(&m_num_runs, "num_runs", "Number of repetitions",
			MS_NOT_AVAILABLE);
	SG_ADD((CSGObject**)&m_xval_outputs, "m_xval_outputs", "List of output "
			"classes for intermediade cross-validation results",
			MS_NOT_AVAILABLE);
}

CEvaluationResult* CCrossValidation::evaluate()
{
	SG_DEBUG("entering %s::evaluate()\n", get_name())

	REQUIRE(m_machine, "%s::evaluate() is only possible if a machine is "
			"attached\n", get_name());

	REQUIRE(m_features, "%s::evaluate() is only possible if features are "
			"attached\n", get_name());

	REQUIRE(m_labels, "%s::evaluate() is only possible if labels are "
			"attached\n", get_name());

	/* if for some reason the do_unlock_frag is set, unlock */
	if (m_do_unlock)
	{
		m_machine->data_unlock();
		m_do_unlock=false;
	}

	/* set labels in any case (no locking needs this) */
	m_machine->set_labels(m_labels);

	if (m_autolock)
	{
		/* if machine supports locking try to do so */
		if (m_machine->supports_locking())
		{
			/* only lock if machine is not yet locked */
			if (!m_machine->is_data_locked())
			{
				m_machine->data_lock(m_labels, m_features);
				m_do_unlock=true;
			}
		}
		else
		{
			SG_WARNING("%s does not support locking. Autolocking is skipped. "
					"Set autolock flag to false to get rid of warning.\n",
					m_machine->get_name());
		}
	}

	SGVector<float64_t> results(m_num_runs);

	/* evtl. update xvalidation output class */
	CCrossValidationOutput* current=(CCrossValidationOutput*)
			m_xval_outputs->get_first_element();
	while (current)
	{
		current->init_num_runs(m_num_runs);
		current->init_num_folds(m_splitting_strategy->get_num_subsets());
		current->init_expose_labels(m_labels);
		current->post_init();
		SG_UNREF(current);
		current=(CCrossValidationOutput*)
				m_xval_outputs->get_next_element();
	}

	/* perform all the x-val runs */
	SG_DEBUG("starting %d runs of cross-validation\n", m_num_runs)
	for (index_t i=0; i <m_num_runs; ++i)
	{

		/* evtl. update xvalidation output class */
		current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
		while (current)
		{
			current->update_run_index(i);
			SG_UNREF(current);
			current=(CCrossValidationOutput*)
					m_xval_outputs->get_next_element();
		}

		SG_DEBUG("entering cross-validation run %d \n", i)
		results[i]=evaluate_one_run();
		SG_DEBUG("result of cross-validation run %d is %f\n", i, results[i])
	}

	/* construct evaluation result */
	CCrossValidationResult* result = new CCrossValidationResult();
	result->mean=CStatistics::mean(results);
	if (m_num_runs>1)
		result->std_dev=CStatistics::std_deviation(results);
	else
		result->std_dev=0;

	/* unlock machine if it was locked in this method */
	if (m_machine->is_data_locked() && m_do_unlock)
	{
		m_machine->data_unlock();
		m_do_unlock=false;
	}

	SG_DEBUG("leaving %s::evaluate()\n", get_name())

	SG_REF(result);
	return result;
}

void CCrossValidation::set_num_runs(int32_t num_runs)
{
	if (num_runs <1)
		SG_ERROR("%d is an illegal number of repetitions\n", num_runs)

	m_num_runs=num_runs;
}

float64_t CCrossValidation::evaluate_one_run()
{
	SG_DEBUG("entering %s::evaluate_one_run()\n", get_name())
	index_t num_subsets=m_splitting_strategy->get_num_subsets();

	SG_DEBUG("building index sets for %d-fold cross-validation\n", num_subsets)

	/* build index sets */
	m_splitting_strategy->build_subsets();

	/* results array */
	SGVector<float64_t> results(num_subsets);

	/* different behavior whether data is locked or not */
	if (m_machine->is_data_locked())
	{
		SG_DEBUG("starting locked evaluation\n", get_name())
		/* do actual cross-validation */
		for (index_t i=0; i <num_subsets; ++i)
		{
			/* evtl. update xvalidation output class */
			CCrossValidationOutput* current=(CCrossValidationOutput*)
					m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_fold_index(i);
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* index subset for training, will be freed below */
			SGVector<index_t> inverse_subset_indices =
					m_splitting_strategy->generate_subset_inverse(i);

			/* train machine on training features */
			m_machine->train_locked(inverse_subset_indices);

			/* feature subset for testing */
			SGVector<index_t> subset_indices =
					m_splitting_strategy->generate_subset_indices(i);

			/* evtl. update xvalidation output class */
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_train_indices(inverse_subset_indices, "\t");
				current->update_trained_machine(m_machine, "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* produce output for desired indices */
			CLabels* result_labels=m_machine->apply_locked(subset_indices);
			SG_REF(result_labels);

			/* set subset for testing labels */
			m_labels->add_subset(subset_indices);

			/* evaluate against own labels */
			m_evaluation_criterion->set_indices(subset_indices);
			results[i]=m_evaluation_criterion->evaluate(result_labels, m_labels);

			/* evtl. update xvalidation output class */
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_test_indices(subset_indices, "\t");
				current->update_test_result(result_labels, "\t");
				current->update_test_true_result(m_labels, "\t");
				current->post_update_results();
				current->update_evaluation_result(results[i], "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* remove subset to prevent side effects */
			m_labels->remove_subset();

			/* clean up */
			SG_UNREF(result_labels);

			SG_DEBUG("done locked evaluation\n", get_name())
		}
	}
	else
	{
		SG_DEBUG("starting unlocked evaluation\n", get_name())
		/* tell machine to store model internally
		 * (otherwise changing subset of features will kaboom the classifier) */
		m_machine->set_store_model_features(true);

		/* do actual cross-validation */
		#pragma omp parallel for
		for (index_t i=0; i <num_subsets; ++i)
		{
			CMachine* machine;
			CFeatures* features;
			CLabels* labels;
			CEvaluation* evaluation_criterion;

			if (get_global_parallel()->get_num_threads()==1)
			{
				machine=m_machine;
				features=m_features;
				evaluation_criterion=m_evaluation_criterion;
			}
			else
			{
				machine=(CMachine*)m_machine->clone();
				features=(CFeatures*)m_features->clone();
				evaluation_criterion=(CEvaluation*)m_evaluation_criterion->clone();
			}

			/* evtl. update xvalidation output class */
			CCrossValidationOutput* current;
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)
					m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_fold_index(i);
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			/* set feature subset for training */
			SGVector<index_t> inverse_subset_indices=
					m_splitting_strategy->generate_subset_inverse(i);

			features->add_subset(inverse_subset_indices);

			/* set label subset for training */
			if (get_global_parallel()->get_num_threads()==1)
				labels=m_labels;
			else
				labels=machine->get_labels();
			labels->add_subset(inverse_subset_indices);

			SG_DEBUG("training set %d:\n", i)
			if (io->get_loglevel()==MSG_DEBUG)
			{
				SGVector<index_t>::display_vector(inverse_subset_indices.vector,
						inverse_subset_indices.vlen, "training indices");
			}

			/* train machine on training features and remove subset */
			SG_DEBUG("starting training\n")
			machine->train(features);
			SG_DEBUG("finished training\n")

			/* evtl. update xvalidation output class */
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_train_indices(inverse_subset_indices, "\t");
				current->update_trained_machine(machine, "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			features->remove_subset();
			labels->remove_subset();

			/* set feature subset for testing (subset method that stores pointer) */
			SGVector<index_t> subset_indices =
					m_splitting_strategy->generate_subset_indices(i);
			features->add_subset(subset_indices);

			/* set label subset for testing */
			labels->add_subset(subset_indices);

			SG_DEBUG("test set %d:\n", i)
			if (io->get_loglevel()==MSG_DEBUG)
			{
				SGVector<index_t>::display_vector(subset_indices.vector,
						subset_indices.vlen, "test indices");
			}

			/* apply machine to test features and remove subset */
			SG_DEBUG("starting evaluation\n")
			SG_DEBUG("%p\n", features)
			CLabels* result_labels=machine->apply(features);
			SG_DEBUG("finished evaluation\n")
			features->remove_subset();
			SG_REF(result_labels);

			/* evaluate */
			results[i]=evaluation_criterion->evaluate(result_labels, labels);
			SG_DEBUG("result on fold %d is %f\n", i, results[i])

			/* evtl. update xvalidation output class */
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_test_indices(subset_indices, "\t");
				current->update_test_result(result_labels, "\t");
				current->update_test_true_result(labels, "\t");
				current->post_update_results();
				current->update_evaluation_result(results[i], "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			/* clean up, remove subsets */
			labels->remove_subset();
			if (get_global_parallel()->get_num_threads()!=1)
			{
				SG_UNREF(machine);
				SG_UNREF(features);
				SG_UNREF(labels);
				SG_UNREF(evaluation_criterion);
			}
			SG_UNREF(result_labels);
		}

		SG_DEBUG("done unlocked evaluation\n", get_name())
	}

	/* build arithmetic mean of results */
	float64_t mean=CStatistics::mean(results);

	SG_DEBUG("leaving %s::evaluate_one_run()\n", get_name())
	return mean;
}

void CCrossValidation::add_cross_validation_output(
			CCrossValidationOutput* cross_validation_output)
{
	m_xval_outputs->append_element(cross_validation_output);
}
<commit_msg>xvalidation back to one thread (temporarily)<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011-2012 Heiko Strathmann
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */

#include <shogun/evaluation/CrossValidation.h>
#include <shogun/machine/Machine.h>
#include <shogun/evaluation/Evaluation.h>
#include <shogun/evaluation/SplittingStrategy.h>
#include <shogun/base/Parameter.h>
#include <shogun/mathematics/Statistics.h>
#include <shogun/evaluation/CrossValidationOutput.h>
#include <shogun/lib/List.h>

using namespace shogun;

CCrossValidation::CCrossValidation() : CMachineEvaluation()
{
	init();
}

CCrossValidation::CCrossValidation(CMachine* machine, CFeatures* features,
		CLabels* labels, CSplittingStrategy* splitting_strategy,
		CEvaluation* evaluation_criterion, bool autolock) :
		CMachineEvaluation(machine, features, labels, splitting_strategy,
		evaluation_criterion, autolock)
{
	init();
}

CCrossValidation::CCrossValidation(CMachine* machine, CLabels* labels,
		CSplittingStrategy* splitting_strategy,
		CEvaluation* evaluation_criterion, bool autolock) :
		CMachineEvaluation(machine, labels, splitting_strategy, evaluation_criterion,
		autolock)
{
	init();
}

CCrossValidation::~CCrossValidation()
{
	SG_UNREF(m_xval_outputs);
}

void CCrossValidation::init()
{
	m_num_runs=1;

	/* do reference counting for output objects */
	m_xval_outputs=new CList(true);

	SG_ADD(&m_num_runs, "num_runs", "Number of repetitions",
			MS_NOT_AVAILABLE);
	SG_ADD((CSGObject**)&m_xval_outputs, "m_xval_outputs", "List of output "
			"classes for intermediade cross-validation results",
			MS_NOT_AVAILABLE);
}

CEvaluationResult* CCrossValidation::evaluate()
{
	SG_DEBUG("entering %s::evaluate()\n", get_name())

	REQUIRE(m_machine, "%s::evaluate() is only possible if a machine is "
			"attached\n", get_name());

	REQUIRE(m_features, "%s::evaluate() is only possible if features are "
			"attached\n", get_name());

	REQUIRE(m_labels, "%s::evaluate() is only possible if labels are "
			"attached\n", get_name());

	/* if for some reason the do_unlock_frag is set, unlock */
	if (m_do_unlock)
	{
		m_machine->data_unlock();
		m_do_unlock=false;
	}

	/* set labels in any case (no locking needs this) */
	m_machine->set_labels(m_labels);

	if (m_autolock)
	{
		/* if machine supports locking try to do so */
		if (m_machine->supports_locking())
		{
			/* only lock if machine is not yet locked */
			if (!m_machine->is_data_locked())
			{
				m_machine->data_lock(m_labels, m_features);
				m_do_unlock=true;
			}
		}
		else
		{
			SG_WARNING("%s does not support locking. Autolocking is skipped. "
					"Set autolock flag to false to get rid of warning.\n",
					m_machine->get_name());
		}
	}

	SGVector<float64_t> results(m_num_runs);

	/* evtl. update xvalidation output class */
	CCrossValidationOutput* current=(CCrossValidationOutput*)
			m_xval_outputs->get_first_element();
	while (current)
	{
		current->init_num_runs(m_num_runs);
		current->init_num_folds(m_splitting_strategy->get_num_subsets());
		current->init_expose_labels(m_labels);
		current->post_init();
		SG_UNREF(current);
		current=(CCrossValidationOutput*)
				m_xval_outputs->get_next_element();
	}

	/* perform all the x-val runs */
	SG_DEBUG("starting %d runs of cross-validation\n", m_num_runs)
	for (index_t i=0; i <m_num_runs; ++i)
	{

		/* evtl. update xvalidation output class */
		current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
		while (current)
		{
			current->update_run_index(i);
			SG_UNREF(current);
			current=(CCrossValidationOutput*)
					m_xval_outputs->get_next_element();
		}

		SG_DEBUG("entering cross-validation run %d \n", i)
		results[i]=evaluate_one_run();
		SG_DEBUG("result of cross-validation run %d is %f\n", i, results[i])
	}

	/* construct evaluation result */
	CCrossValidationResult* result = new CCrossValidationResult();
	result->mean=CStatistics::mean(results);
	if (m_num_runs>1)
		result->std_dev=CStatistics::std_deviation(results);
	else
		result->std_dev=0;

	/* unlock machine if it was locked in this method */
	if (m_machine->is_data_locked() && m_do_unlock)
	{
		m_machine->data_unlock();
		m_do_unlock=false;
	}

	SG_DEBUG("leaving %s::evaluate()\n", get_name())

	SG_REF(result);
	return result;
}

void CCrossValidation::set_num_runs(int32_t num_runs)
{
	if (num_runs <1)
		SG_ERROR("%d is an illegal number of repetitions\n", num_runs)

	m_num_runs=num_runs;
}

float64_t CCrossValidation::evaluate_one_run()
{
	SG_DEBUG("entering %s::evaluate_one_run()\n", get_name())
	index_t num_subsets=m_splitting_strategy->get_num_subsets();

	SG_DEBUG("building index sets for %d-fold cross-validation\n", num_subsets)

	/* build index sets */
	m_splitting_strategy->build_subsets();

	/* results array */
	SGVector<float64_t> results(num_subsets);

	/* different behavior whether data is locked or not */
	if (m_machine->is_data_locked())
	{
		SG_DEBUG("starting locked evaluation\n", get_name())
		/* do actual cross-validation */
		for (index_t i=0; i <num_subsets; ++i)
		{
			/* evtl. update xvalidation output class */
			CCrossValidationOutput* current=(CCrossValidationOutput*)
					m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_fold_index(i);
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* index subset for training, will be freed below */
			SGVector<index_t> inverse_subset_indices =
					m_splitting_strategy->generate_subset_inverse(i);

			/* train machine on training features */
			m_machine->train_locked(inverse_subset_indices);

			/* feature subset for testing */
			SGVector<index_t> subset_indices =
					m_splitting_strategy->generate_subset_indices(i);

			/* evtl. update xvalidation output class */
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_train_indices(inverse_subset_indices, "\t");
				current->update_trained_machine(m_machine, "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* produce output for desired indices */
			CLabels* result_labels=m_machine->apply_locked(subset_indices);
			SG_REF(result_labels);

			/* set subset for testing labels */
			m_labels->add_subset(subset_indices);

			/* evaluate against own labels */
			m_evaluation_criterion->set_indices(subset_indices);
			results[i]=m_evaluation_criterion->evaluate(result_labels, m_labels);

			/* evtl. update xvalidation output class */
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_test_indices(subset_indices, "\t");
				current->update_test_result(result_labels, "\t");
				current->update_test_true_result(m_labels, "\t");
				current->post_update_results();
				current->update_evaluation_result(results[i], "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}

			/* remove subset to prevent side effects */
			m_labels->remove_subset();

			/* clean up */
			SG_UNREF(result_labels);

			SG_DEBUG("done locked evaluation\n", get_name())
		}
	}
	else
	{
		SG_DEBUG("starting unlocked evaluation\n", get_name())
		/* tell machine to store model internally
		 * (otherwise changing subset of features will kaboom the classifier) */
		m_machine->set_store_model_features(true);

		/* do actual cross-validation */

		//TODO parallel xvalidation needs some serious fixing, see #3743
		//#pragma omp parallel for
		for (index_t i=0; i <num_subsets; ++i)
		{
			CMachine* machine;
			CFeatures* features;
			CLabels* labels;
			CEvaluation* evaluation_criterion;

			if (get_global_parallel()->get_num_threads()==1)
			{
				machine=m_machine;
				features=m_features;
				evaluation_criterion=m_evaluation_criterion;
			}
			else
			{
				machine=(CMachine*)m_machine->clone();
				features=(CFeatures*)m_features->clone();
				evaluation_criterion=(CEvaluation*)m_evaluation_criterion->clone();
			}

			/* evtl. update xvalidation output class */
			CCrossValidationOutput* current;
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)
					m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_fold_index(i);
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			/* set feature subset for training */
			SGVector<index_t> inverse_subset_indices=
					m_splitting_strategy->generate_subset_inverse(i);

			features->add_subset(inverse_subset_indices);

			/* set label subset for training */
			if (get_global_parallel()->get_num_threads()==1)
				labels=m_labels;
			else
				labels=machine->get_labels();
			labels->add_subset(inverse_subset_indices);

			SG_DEBUG("training set %d:\n", i)
			if (io->get_loglevel()==MSG_DEBUG)
			{
				SGVector<index_t>::display_vector(inverse_subset_indices.vector,
						inverse_subset_indices.vlen, "training indices");
			}

			/* train machine on training features and remove subset */
			SG_DEBUG("starting training\n")
			machine->train(features);
			SG_DEBUG("finished training\n")

			/* evtl. update xvalidation output class */
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_train_indices(inverse_subset_indices, "\t");
				current->update_trained_machine(machine, "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			features->remove_subset();
			labels->remove_subset();

			/* set feature subset for testing (subset method that stores pointer) */
			SGVector<index_t> subset_indices =
					m_splitting_strategy->generate_subset_indices(i);
			features->add_subset(subset_indices);

			/* set label subset for testing */
			labels->add_subset(subset_indices);

			SG_DEBUG("test set %d:\n", i)
			if (io->get_loglevel()==MSG_DEBUG)
			{
				SGVector<index_t>::display_vector(subset_indices.vector,
						subset_indices.vlen, "test indices");
			}

			/* apply machine to test features and remove subset */
			SG_DEBUG("starting evaluation\n")
			SG_DEBUG("%p\n", features)
			CLabels* result_labels=machine->apply(features);
			SG_DEBUG("finished evaluation\n")
			features->remove_subset();
			SG_REF(result_labels);

			/* evaluate */
			results[i]=evaluation_criterion->evaluate(result_labels, labels);
			SG_DEBUG("result on fold %d is %f\n", i, results[i])

			/* evtl. update xvalidation output class */
			#pragma omp critical
			{
			current=(CCrossValidationOutput*)m_xval_outputs->get_first_element();
			while (current)
			{
				current->update_test_indices(subset_indices, "\t");
				current->update_test_result(result_labels, "\t");
				current->update_test_true_result(labels, "\t");
				current->post_update_results();
				current->update_evaluation_result(results[i], "\t");
				SG_UNREF(current);
				current=(CCrossValidationOutput*)
						m_xval_outputs->get_next_element();
			}
			}

			/* clean up, remove subsets */
			labels->remove_subset();
			if (get_global_parallel()->get_num_threads()!=1)
			{
				SG_UNREF(machine);
				SG_UNREF(features);
				SG_UNREF(labels);
				SG_UNREF(evaluation_criterion);
			}
			SG_UNREF(result_labels);
		}

		SG_DEBUG("done unlocked evaluation\n", get_name())
	}

	/* build arithmetic mean of results */
	float64_t mean=CStatistics::mean(results);

	SG_DEBUG("leaving %s::evaluate_one_run()\n", get_name())
	return mean;
}

void CCrossValidation::add_cross_validation_output(
			CCrossValidationOutput* cross_validation_output)
{
	m_xval_outputs->append_element(cross_validation_output);
}
<|endoftext|>


<commit_before>#include <QGuiApplication>
#include <QQmlApplicationEngine>
#include <QtQml/QQmlPropertyMap>
#include <QString>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <taglib/taglib.h>
#include <taglib/tag.h>
#include <taglib/fileref.h>
#include <taglib/taglib_config.h>
#include <taglib/tpropertymap.h>
#include <qdebug.h>
#include <qqml.h>
#include <QQuickView>
#include <QDateTime>
#include <QQmlContext>
#include <QFileInfo>
#include <QDir>
#include <QtSql/QSql>
#include <QtSql/QSqlDatabase>
#include <QtSql/QSqlQuery>
#include <QList>
#include <QObject>
#include <main.h>

QList<QObject*> getAllSongs(QSqlDatabase db){
    QList<QObject*> songList;
    //songList.append(new SongObject());

    if(db.open()){
        // DB is open; lets get all songs
        QSqlQuery getSongs;
        getSongs.prepare("Select * FROM Songs");
        if(getSongs.exec()){
            while(getSongs.next()){
                QString title = getSongs.value(2).toString();
                QString path = getSongs.value(1).toString();
                QString album = getSongs.value(4).toString();
                QString artist = getSongs.value(3).toString();

                //songList.append(new SongObject(getSongs.value(2).fromValue, getSongs.value(1).fromValue, getSongs.value(4).fromValue, getSongs.value(3).fromValue));
            }
        }
    }
    return songList;
}

void addSongsToDatabase(TagLib::String path, QString newpath, QSqlDatabase db){
    // Get TagLib instance of song by path
    TagLib::FileRef f(path.toCString());
    TagLib::Tag *tag = f.tag();
    // std::cout << "Adding " << tag->title() << " " << tag->artist() << " to the database." << std::endl;
    QString artist = tag->artist().toCString();
    QString title = tag->title().toCString();
    QString album = tag->album().toCString();

    if(db.open()){
        QSqlQuery qry;
        qry.prepare("CREATE TABLE IF NOT EXISTS Songs(id INTEGER PRIMARY KEY AUTOINCREMENT, path TEXT, title TEXT, artist TEXT, album TEXT)");

        if(qry.exec()){
            //std::cout << "Created table" << std::endl;
        }

        // Check if item already exists for this path
        QSqlQuery getSong;
        getSong.prepare("SELECT * FROM Songs WHERE path='" + newpath + "'");
        if(getSong.exec()){
            // If .first == true, it already exists
           if(getSong.first()){
                while(getSong.next()){
                    //std::cout << getSong.value(1).toString().toUtf8().constData() << std::endl;
                }
            }else{
                // If not, we add it.
                QSqlQuery insertqry;
                insertqry.prepare("INSERT INTO Songs (id, path, title, artist, album) VALUES "
                                  "(NULL, '"+ newpath +"', '"+ title +"', '"+ artist +"', '"+ album +"')");
                if(insertqry.exec()){
                    std::cout << "Inserted data successfully" << std::endl;
                }else{
                     std::cout << "Data not inserted" << std::endl;
                }
            }
        }

    }else{
        // If we couldn't open the database it means the entire program is broken and will need a rewrite
        //std::cout << "Couldnt open database" << std::endl;
    }
}

void firstMusicScan(QDir d, QSqlDatabase db, bool recursive=true, bool symlinks=false ) {
    // Get top directory (~/Music by default), and recursively scan for mp3
    d.setSorting( QDir::Name );
    QDir::Filters df = QDir::Files | QDir::NoDotAndDotDot;
    if (recursive) df |= QDir::Dirs;
    if (not symlinks) df |= QDir::NoSymLinks;
    QStringList qsl = d.entryList(df, QDir::Name | QDir::DirsFirst);

    // For each: if folder, run this method, if mp3, add to DB
    foreach (const QString &entry, qsl) {
        QFileInfo finfo(d, entry);
        if ( finfo.isDir() ) {
            QDir sd(finfo.absoluteFilePath());
            firstMusicScan(sd, db);
        } else {
            if (finfo.completeSuffix()=="mp3")
                addSongsToDatabase(finfo.absoluteFilePath().toStdString(), QString(finfo.absoluteFilePath()), db);
        }
    }
}

void initialQuery(){
    // Open DB connection
    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setHostName("localhost");
    db.setDatabaseName("vinylmusic");
    std::cout << "Loading database data. " << std::endl;

    if(db.open()){
        // Create Settings table if it isnt there
        QSqlQuery qry;
        qry.prepare("CREATE TABLE IF NOT EXISTS Settings(id INTEGER PRIMARY KEY AUTOINCREMENT, setting TEXT, value TEXT)");
        if(qry.exec()){
            std::cout << "Created table" << std::endl;
        }else{
            std::cout << "Didnt create table" << std::endl;
        }
    }else{
        std::cout << "Couldnt open database" << std::endl;
    }

    // Uncomment for debug purposes
    /*
    QSqlQuery getSettings;
    getSettings.prepare("Select * FROM Settings");
    if(getSettings.exec()){
        while(getSettings.next()){
            std::cout << getSettings.value(1).toString().toUtf8().constData() << std::endl;
        }
    }
    */
}

int main(int argc, char *argv[]){

    QGuiApplication app(argc, argv);
    QQmlApplicationEngine engine;

    if (argv[1]) {
        // If Vinyl Music was opened with a song path:
        // This is a real dirty way to do things....
        QFileInfo finfo;
        finfo.setFile(argv[1]);
        engine.rootContext()->setContextProperty("loadedFileFolder", QString(finfo.path()));
    }

    // Initialize the database if not exists, and set Settings table
    initialQuery();

    // Open DB and perform initial music dir scan/build Songs table
    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setHostName("localhost");
    db.setDatabaseName("vinylmusic");
    std::cout << "Loading database data. " << std::endl;
    firstMusicScan(QDir::homePath() + "/Music", db);

    // Get ~/Music directory and ~/Music/streams directory
    std::string home_directory = QDir::homePath().toStdString() + std::string("/Music");
    std::string stream_directory = QDir::homePath().toStdString() + std::string("/Music/streams");

    // Create path variables accessible in QML:
    engine.rootContext()->setContextProperty("homeDirectory", QString::fromStdString(home_directory));
    engine.rootContext()->setContextProperty("streamDirectory", QString::fromStdString(stream_directory));
    engine.rootContext()->setContextProperty("allSongObjects", QVariant::fromValue(getAllSongs(db)));

    // Create view from main.qml:
    engine.load(QUrl(QStringLiteral("qrc:/qml/main.qml")));

    // Run the app
    return app.exec();
}

<commit_msg>Use QStandardPaths to resolve music folder. Also fix cli args<commit_after>#include <QGuiApplication>
#include <QQmlApplicationEngine>
#include <QtQml/QQmlPropertyMap>
#include <QString>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <taglib/taglib.h>
#include <taglib/tag.h>
#include <taglib/fileref.h>
#include <taglib/taglib_config.h>
#include <taglib/tpropertymap.h>
#include <qdebug.h>
#include <qqml.h>
#include <QQuickView>
#include <QDateTime>
#include <QQmlContext>
#include <QFileInfo>
#include <QDir>
#include <QtSql/QSql>
#include <QtSql/QSqlDatabase>
#include <QtSql/QSqlQuery>
#include <QList>
#include <QObject>
#include <QStandardPaths>
#include <main.h>

QList<QObject*> getAllSongs(QSqlDatabase db){
    QList<QObject*> songList;
    //songList.append(new SongObject());

    if(db.open()){
        // DB is open; lets get all songs
        QSqlQuery getSongs;
        getSongs.prepare("Select * FROM Songs");
        if(getSongs.exec()){
            while(getSongs.next()){
                QString title = getSongs.value(2).toString();
                QString path = getSongs.value(1).toString();
                QString album = getSongs.value(4).toString();
                QString artist = getSongs.value(3).toString();

                //songList.append(new SongObject(getSongs.value(2).fromValue, getSongs.value(1).fromValue, getSongs.value(4).fromValue, getSongs.value(3).fromValue));
            }
        }
    }
    return songList;
}

void addSongsToDatabase(TagLib::String path, QString newpath, QSqlDatabase db){
    // Get TagLib instance of song by path
    TagLib::FileRef f(path.toCString());
    TagLib::Tag *tag = f.tag();
    // std::cout << "Adding " << tag->title() << " " << tag->artist() << " to the database." << std::endl;
    QString artist = tag->artist().toCString();
    QString title = tag->title().toCString();
    QString album = tag->album().toCString();

    if(db.open()){
        QSqlQuery qry;
        qry.prepare("CREATE TABLE IF NOT EXISTS Songs(id INTEGER PRIMARY KEY AUTOINCREMENT, path TEXT, title TEXT, artist TEXT, album TEXT)");

        if(qry.exec()){
            //std::cout << "Created table" << std::endl;
        }

        // Check if item already exists for this path
        QSqlQuery getSong;
        getSong.prepare("SELECT * FROM Songs WHERE path='" + newpath + "'");
        if(getSong.exec()){
            // If .first == true, it already exists
           if(getSong.first()){
                while(getSong.next()){
                    //std::cout << getSong.value(1).toString().toUtf8().constData() << std::endl;
                }
            }else{
                // If not, we add it.
                QSqlQuery insertqry;
                insertqry.prepare("INSERT INTO Songs (id, path, title, artist, album) VALUES "
                                  "(NULL, '"+ newpath +"', '"+ title +"', '"+ artist +"', '"+ album +"')");
                if(insertqry.exec()){
                    std::cout << "Inserted data successfully" << std::endl;
                }else{
                     std::cout << "Data not inserted" << std::endl;
                }
            }
        }

    }else{
        // If we couldn't open the database it means the entire program is broken and will need a rewrite
        //std::cout << "Couldnt open database" << std::endl;
    }
}

void firstMusicScan(QDir d, QSqlDatabase db, bool recursive=true, bool symlinks=false ) {
    // Get top directory (~/Music by default), and recursively scan for mp3
    d.setSorting( QDir::Name );
    QDir::Filters df = QDir::Files | QDir::NoDotAndDotDot;
    if (recursive) df |= QDir::Dirs;
    if (not symlinks) df |= QDir::NoSymLinks;
    QStringList qsl = d.entryList(df, QDir::Name | QDir::DirsFirst);

    // For each: if folder, run this method, if mp3, add to DB
    foreach (const QString &entry, qsl) {
        QFileInfo finfo(d, entry);
        if ( finfo.isDir() ) {
            QDir sd(finfo.absoluteFilePath());
            firstMusicScan(sd, db);
        } else {
            if (finfo.completeSuffix()=="mp3")
                addSongsToDatabase(finfo.absoluteFilePath().toStdString(), QString(finfo.absoluteFilePath()), db);
        }
    }
}

void initialQuery(){
    // Open DB connection
    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setHostName("localhost");
    db.setDatabaseName("vinylmusic");
    std::cout << "Loading database data. " << std::endl;

    if(db.open()){
        // Create Settings table if it isnt there
        QSqlQuery qry;
        qry.prepare("CREATE TABLE IF NOT EXISTS Settings(id INTEGER PRIMARY KEY AUTOINCREMENT, setting TEXT, value TEXT)");
        if(qry.exec()){
            std::cout << "Created table" << std::endl;
        }else{
            std::cout << "Didnt create table" << std::endl;
        }
    }else{
        std::cout << "Couldnt open database" << std::endl;
    }

    // Uncomment for debug purposes
    /*
    QSqlQuery getSettings;
    getSettings.prepare("Select * FROM Settings");
    if(getSettings.exec()){
        while(getSettings.next()){
            std::cout << getSettings.value(1).toString().toUtf8().constData() << std::endl;
        }
    }
    */
}

int main(int argc, char *argv[]){

    QGuiApplication app(argc, argv);
    QQmlApplicationEngine engine;

    if (app.arguments().size() > 1) {
        // If Vinyl Music was opened with a song path:
        QFileInfo finfo;
        finfo.setFile(app.arguments().at(1));
        engine.rootContext()->setContextProperty("loadedFileFolder", QString(finfo.path()));
    }

    // Initialize the database if not exists, and set Settings table
    initialQuery();

    const QStringList musicLocations = QStandardPaths::standardLocations(QStandardPaths::MusicLocation);
    QString musicLocation = musicLocations.isEmpty() ?
                QDir::homePath() + QLatin1String("/Music") : musicLocations.first();

    // Open DB and perform initial music dir scan/build Songs table
    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setHostName("localhost");
    db.setDatabaseName("vinylmusic");
    std::cout << "Loading database data. " << std::endl;
    firstMusicScan(QDir(musicLocation), db);

    QString stream_directory = musicLocation + QLatin1String("/streams");

    // Create path variables accessible in QML:
    engine.rootContext()->setContextProperty("homeDirectory", musicLocation);
    engine.rootContext()->setContextProperty("streamDirectory", stream_directory);
    engine.rootContext()->setContextProperty("allSongObjects", QVariant::fromValue(getAllSongs(db)));

    // Create view from main.qml:
    engine.load(QUrl(QStringLiteral("qrc:/qml/main.qml")));

    // Run the app
    return app.exec();
}

<|endoftext|>


<commit_before>/* This MUST all be at the top */
#include "builtin/fiber.hpp"
/* end must be at the top */

#include "builtin/object.hpp"
#include "vm/vm.hpp"
#include "builtin/class.hpp"
#include "builtin/integer.hpp"

#include "objectmemory.hpp"
#include "gc/gc.hpp"

#include "call_frame.hpp"
#include "arguments.hpp"

namespace rubinius {

  void Fiber::init(STATE) {
    GO(fiber).set(state->new_class("Fiber", G(object), G(rubinius)));
    G(fiber)->set_object_type(state, FiberType);
  }

  Fiber* Fiber::current(STATE) {
#ifdef FIBER_ENABLED
    Fiber* fib = state->current_fiber.get();

    // Lazily allocate a root fiber.
    if(fib->nil_p()) {
      fib = state->new_object<Fiber>(G(fiber));
      fib->prev_ = (Fiber*)Qnil;
      fib->top_ = 0;
      fib->root_ = true;
      fib->status_ = Fiber::eRunning;
      fib->state_ = state;
      fib->stack_size_ = state->stack_size();
      fib->stack_ = state->stack_start();
      fib->context_ = new ucontext_t;

      state->om->needs_finalization(fib);

      state->current_fiber.set(fib);
    }

    return fib;
#else
    return Qnil;
#endif
  }

  void Fiber::start_on_stack() {
#ifdef FIBER_ENABLED
    VM* state = VM::current();

    Fiber* fib = Fiber::current(state);

    // Affix this fiber to this thread now.
    fib->state_ = state;

    fib->starter()->send(state, NULL, G(sym_call));
    // GC has run! Don't use stack vars!

    fib = Fiber::current(state);
    fib->status_ = Fiber::eDead;

    Fiber* dest = fib->prev();
    assert(!dest->nil_p());

    dest->run();
    dest->value(state, Qnil);
    state->set_current_fiber(dest);

    if(setcontext(dest->ucontext()) != 0)
      assert(0 && "fatal swapcontext() error");

    assert(0 && "fatal start_on_stack error");
#else
    abort();
#endif
  }

  Fiber* Fiber::create(STATE, Integer* i_stack_size, Object* callable) {
#ifdef FIBER_ENABLED
    int stack_size = i_stack_size->to_native();

    if(stack_size < 64 * 1024) {
      stack_size = 64 * 1024;
    }

    Fiber* fib = state->new_object<Fiber>(G(fiber));
    fib->starter(state, callable);
    fib->prev_ = (Fiber*)Qnil;
    fib->top_ = 0;
    fib->root_ = false;
    fib->state_ = 0;
    fib->status_ = Fiber::eSleeping;
    fib->stack_size_ = stack_size;
    fib->stack_ = malloc(stack_size);
    fib->context_ = new ucontext_t;

    state->om->needs_finalization(fib);

    ucontext_t* ctx = fib->ucontext();

    if(getcontext(ctx) != 0) assert(0 && "fatal getcontext() error");

    ctx->uc_link = 0;
    ctx->uc_stack.ss_sp = fib->stack_;
    ctx->uc_stack.ss_size = stack_size;
    ctx->uc_stack.ss_flags = 0;

    makecontext(ctx, start_on_stack, 0);

    return fib;
#else
    return Primitives::failure();
#endif
  }

  Object* Fiber::resume(STATE, Arguments& args, CallFrame* calling_environment) {
#ifdef FIBER_ENABLED
    if(!prev_->nil_p() || root_) return Primitives::failure();

    Object* val = Qnil;
    if(args.total() == 1) {
      val = args.get_argument(0);
    } else if(args.total() > 1) {
      val = args.as_array(state);
    }

    value(state, val);

    Fiber* cur = Fiber::current(state);
    prev(state, cur);

    cur->sleep(calling_environment);

    run();
    state->set_current_fiber(this);

    if(swapcontext(cur->ucontext(), context_) != 0)
      assert(0 && "fatal swapcontext() error");

    // Back here when someone yields back to us!
    // Beware here, because the GC has probably run so GC pointers on the C++ stack
    // can't be accessed.

    cur = Fiber::current(state);
    return cur->value();
#else
    return Primitives::failure();
#endif
  }

  Object* Fiber::s_yield(STATE, Arguments& args, CallFrame* calling_environment) {
#ifdef FIBER_ENABLED
    Fiber* cur = Fiber::current(state);
    Fiber* dest_fib = cur->prev();

    assert(cur != dest_fib);

    cur->prev(state, (Fiber*)Qnil);

    Object* val = Qnil;
    if(args.total() == 1) {
      val = args.get_argument(0);
    } else if(args.total() > 1) {
      val = args.as_array(state);
    }

    dest_fib->value(state, val);

    cur->sleep(calling_environment);

    dest_fib->run();
    state->set_current_fiber(dest_fib);

    if(swapcontext(cur->ucontext(), dest_fib->ucontext()) != 0)
      assert(0 && "fatal swapcontext() error");

    // Back here when someone yields back to us!
    // Beware here, because the GC has probably run so GC pointers on the C++ stack
    // can't be accessed.

    cur = Fiber::current(state);
    return cur->value();
#else
    return Primitives::failure();
#endif
  }

  void Fiber::finalize(STATE) {
#ifdef FIBER_ENABLED
    delete context_;
    if(stack_ && !root_) free(stack_);
#endif
  }

  void Fiber::Info::mark(Object* obj, ObjectMark& mark) {
    auto_mark(obj, mark);

    Fiber* fib = (Fiber*)obj;
    if(CallFrame* cf = fib->call_frame()) {
      mark.gc->walk_call_frame(cf);
    }
  }
}
<commit_msg>Add explicit casts when Fiber isn't supported<commit_after>/* This MUST all be at the top */
#include "builtin/fiber.hpp"
/* end must be at the top */

#include "builtin/object.hpp"
#include "vm/vm.hpp"
#include "builtin/class.hpp"
#include "builtin/integer.hpp"

#include "objectmemory.hpp"
#include "gc/gc.hpp"

#include "call_frame.hpp"
#include "arguments.hpp"

namespace rubinius {

  void Fiber::init(STATE) {
    GO(fiber).set(state->new_class("Fiber", G(object), G(rubinius)));
    G(fiber)->set_object_type(state, FiberType);
  }

  Fiber* Fiber::current(STATE) {
#ifdef FIBER_ENABLED
    Fiber* fib = state->current_fiber.get();

    // Lazily allocate a root fiber.
    if(fib->nil_p()) {
      fib = state->new_object<Fiber>(G(fiber));
      fib->prev_ = reinterpret_cast<Fiber*>(Qnil);
      fib->top_ = 0;
      fib->root_ = true;
      fib->status_ = Fiber::eRunning;
      fib->state_ = state;
      fib->stack_size_ = state->stack_size();
      fib->stack_ = state->stack_start();
      fib->context_ = new ucontext_t;

      state->om->needs_finalization(fib);

      state->current_fiber.set(fib);
    }

    return fib;
#else
    return reinterpret_cast<Fiber*>(Qnil);
#endif
  }

  void Fiber::start_on_stack() {
#ifdef FIBER_ENABLED
    VM* state = VM::current();

    Fiber* fib = Fiber::current(state);

    // Affix this fiber to this thread now.
    fib->state_ = state;

    fib->starter()->send(state, NULL, G(sym_call));
    // GC has run! Don't use stack vars!

    fib = Fiber::current(state);
    fib->status_ = Fiber::eDead;

    Fiber* dest = fib->prev();
    assert(!dest->nil_p());

    dest->run();
    dest->value(state, Qnil);
    state->set_current_fiber(dest);

    if(setcontext(dest->ucontext()) != 0)
      assert(0 && "fatal swapcontext() error");

    assert(0 && "fatal start_on_stack error");
#else
    abort();
#endif
  }

  Fiber* Fiber::create(STATE, Integer* i_stack_size, Object* callable) {
#ifdef FIBER_ENABLED
    int stack_size = i_stack_size->to_native();

    if(stack_size < 64 * 1024) {
      stack_size = 64 * 1024;
    }

    Fiber* fib = state->new_object<Fiber>(G(fiber));
    fib->starter(state, callable);
    fib->prev_ = (Fiber*)Qnil;
    fib->top_ = 0;
    fib->root_ = false;
    fib->state_ = 0;
    fib->status_ = Fiber::eSleeping;
    fib->stack_size_ = stack_size;
    fib->stack_ = malloc(stack_size);
    fib->context_ = new ucontext_t;

    state->om->needs_finalization(fib);

    ucontext_t* ctx = fib->ucontext();

    if(getcontext(ctx) != 0) assert(0 && "fatal getcontext() error");

    ctx->uc_link = 0;
    ctx->uc_stack.ss_sp = fib->stack_;
    ctx->uc_stack.ss_size = stack_size;
    ctx->uc_stack.ss_flags = 0;

    makecontext(ctx, start_on_stack, 0);

    return fib;
#else
    return reinterpret_cast<Fiber*>(Primitives::failure());
#endif
  }

  Object* Fiber::resume(STATE, Arguments& args, CallFrame* calling_environment) {
#ifdef FIBER_ENABLED
    if(!prev_->nil_p() || root_) return Primitives::failure();

    Object* val = Qnil;
    if(args.total() == 1) {
      val = args.get_argument(0);
    } else if(args.total() > 1) {
      val = args.as_array(state);
    }

    value(state, val);

    Fiber* cur = Fiber::current(state);
    prev(state, cur);

    cur->sleep(calling_environment);

    run();
    state->set_current_fiber(this);

    if(swapcontext(cur->ucontext(), context_) != 0)
      assert(0 && "fatal swapcontext() error");

    // Back here when someone yields back to us!
    // Beware here, because the GC has probably run so GC pointers on the C++ stack
    // can't be accessed.

    cur = Fiber::current(state);
    return cur->value();
#else
    return Primitives::failure();
#endif
  }

  Object* Fiber::s_yield(STATE, Arguments& args, CallFrame* calling_environment) {
#ifdef FIBER_ENABLED
    Fiber* cur = Fiber::current(state);
    Fiber* dest_fib = cur->prev();

    assert(cur != dest_fib);

    cur->prev(state, (Fiber*)Qnil);

    Object* val = Qnil;
    if(args.total() == 1) {
      val = args.get_argument(0);
    } else if(args.total() > 1) {
      val = args.as_array(state);
    }

    dest_fib->value(state, val);

    cur->sleep(calling_environment);

    dest_fib->run();
    state->set_current_fiber(dest_fib);

    if(swapcontext(cur->ucontext(), dest_fib->ucontext()) != 0)
      assert(0 && "fatal swapcontext() error");

    // Back here when someone yields back to us!
    // Beware here, because the GC has probably run so GC pointers on the C++ stack
    // can't be accessed.

    cur = Fiber::current(state);
    return cur->value();
#else
    return Primitives::failure();
#endif
  }

  void Fiber::finalize(STATE) {
#ifdef FIBER_ENABLED
    delete context_;
    if(stack_ && !root_) free(stack_);
#endif
  }

  void Fiber::Info::mark(Object* obj, ObjectMark& mark) {
    auto_mark(obj, mark);

    Fiber* fib = (Fiber*)obj;
    if(CallFrame* cf = fib->call_frame()) {
      mark.gc->walk_call_frame(cf);
    }
  }
}
<|endoftext|>


<commit_before>//
// Copyright (c) 2017 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/steering-method/constant-curvature.hh>

#include <pinocchio/spatial/se3.hpp>

#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup.hh>

namespace hpp {
  namespace core {
    namespace steeringMethod {

      ConstantCurvaturePtr_t ConstantCurvature::create
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
       const ConstraintSetPtr_t& constraints)
      {
        ConstantCurvature* ptr;
        if (constraints)
          ptr = new ConstantCurvature (
              robot, init, end, curveLength, pathLength,
              curvature, xyId, rzId, rz, wheels, constraints);
        else
          ptr = new ConstantCurvature (
              robot, init, end, curveLength, pathLength,
              curvature, xyId, rzId, rz, wheels);
        ConstantCurvaturePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      ConstantCurvaturePtr_t ConstantCurvature::createCopy
      (const ConstantCurvaturePtr_t& other)
      {
        ConstantCurvature* ptr (new ConstantCurvature (*other));
        ConstantCurvaturePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      ConstantCurvaturePtr_t ConstantCurvature::createCopy
      (const ConstantCurvaturePtr_t& other,
       const ConstraintSetPtr_t& constraints)
      {
	ConstantCurvature* ptr = new ConstantCurvature (*other, constraints);
	ConstantCurvaturePtr_t shPtr (ptr);
	ptr->init (shPtr);
	return shPtr;
      }

      /// Return a shared pointer to a copy of this
      PathPtr_t ConstantCurvature::copy () const
      {
	return createCopy (weak_.lock ());
      }

      std::ostream& ConstantCurvature::print (std::ostream &os) const
      {
        os << "-- ConstantCurvature" << std::endl;
        os << "from " << initial_.transpose () << std::endl;
        os << "  length: " << forward_ * paramLength() << std::endl;
        os << "  curvature: " << curvature_ << std::endl;
        return os;
      }

      ConstantCurvature::ConstantCurvature
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
       ConstraintSetPtr_t constraints) :
        Path (std::make_pair (0., fabs (pathLength)), robot->configSize (),
              robot->numberDof (), constraints), robot_ (robot),
        initial_ (init), end_ (end),
        curveLength_ (curveLength), curvature_ (curvature),
        xyId_ (xyId), rzId_ (rzId), forward_ (curveLength > 0 ? 1 : -1)
      {
        // Find rank of translation and rotation in velocity vectors
        // Hypothesis: degrees of freedom all belong to a planar joint or
        // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
        JointPtr_t joint (robot_->getJointAtConfigRank (xyId_));
        size_type offset (xyId_ - joint->rankInConfiguration ());
        dxyId_ = joint->rankInVelocity () + offset;
        joint = robot_->getJointAtConfigRank (rzId_);
        offset = rzId_ - joint->rankInConfiguration ();
        drzId_ = joint->rankInVelocity () + offset;
        setWheelJoints (rz, wheels);
        impl_compute (end_, paramRange ().second);
      }

      ConstantCurvature::ConstantCurvature
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels) :
        Path (std::make_pair (0., fabs (pathLength)), robot->configSize (),
              robot->numberDof ()), robot_ (robot),
        initial_ (init), end_ (end), curveLength_ (curveLength),
        curvature_ (curvature), xyId_ (xyId),
        rzId_ (rzId), forward_ (curveLength > 0 ? 1 : -1)
      {
        // Find rank of translation and rotation in velocity vectors
        // Hypothesis: degrees of freedom all belong to a planar joint or
        // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
        JointPtr_t joint (robot_->getJointAtConfigRank (xyId_));
        size_type offset (xyId_ - joint->rankInConfiguration ());
        dxyId_ = joint->rankInVelocity () + offset;
        joint = robot_->getJointAtConfigRank (rzId_);
        offset = rzId_ - joint->rankInConfiguration ();
        drzId_ = joint->rankInVelocity () + offset;
        setWheelJoints (rz, wheels);
        impl_compute (end_, paramRange ().second);
      }

      ConstantCurvature::ConstantCurvature (const ConstantCurvature& other) :
        Path (other), robot_ (other.robot_), initial_ (other.initial_),
        end_ (other.end_), curveLength_ (other.curveLength_),
        curvature_ (other.curvature_),  xyId_ (other.xyId_),
        rzId_ (other.rzId_), forward_ (other.forward_), wheels_ (other.wheels_)
      {
      }

      ConstantCurvature::ConstantCurvature
      (const ConstantCurvature& other, const ConstraintSetPtr_t& constraints) :
        parent_t (other, constraints), robot_ (other.robot_),
        initial_ (other.initial_), end_ (other.end_),
        curveLength_ (other.curveLength_), curvature_ (other.curvature_),
        xyId_ (other.xyId_), rzId_ (other.rzId_), forward_ (other.forward_),
        wheels_ (other.wheels_)
      {
      }

      bool ConstantCurvature::impl_compute (ConfigurationOut_t result,
                                            value_type param) const
      {
        const value_type L = paramLength();
        // Does a linear interpolation on all the joints.
        const value_type u = (L == 0) ? 0 : ((param - paramRange ().first) / L );
        pinocchio::interpolate <pinocchio::RnxSOnLieGroupMap>(robot_, initial_, end_, u, result);

        value_type t (u * curveLength_);
        value_type x0 (initial_ [xyId_ + 0]), y0 (initial_ [xyId_ + 1]);
        value_type c0 (initial_ [rzId_ + 0]), s0 (initial_ [rzId_ + 1]);
        value_type x, y;
        value_type c (cos (curvature_ * t)), s (sin (curvature_ * t));

        if (curvature_ == 0) {
          x  = x0 + t * c0;
          y  = y0 + t * s0;
        } else {
          value_type r (1./curvature_);
          x = x0 + r * (s0 * (c - 1) + c0 * s);
          y = y0 + r * (c0 * (1 - c) + s0 * s);
        }
        result [xyId_ + 0] = x;
        result [xyId_ + 1] = y;
        result [rzId_ + 0] = c0 * c - s0 * s;
        result [rzId_ + 1] = c0 * s + s0 * c;

        // Set wheel joint positions
        for (std::vector<Wheels_t>::const_iterator w = wheels_.begin ();
             w < wheels_.end (); ++w) {
          result [w->j->rankInConfiguration ()] = w->value;
        }
        return true;
      }

      void ConstantCurvature::impl_derivative
      (vectorOut_t result, const value_type& param, size_type order) const
      {
        const value_type L = paramLength();
        // Does a linear interpolation on all the joints.
        const value_type u = (L == 0) ? 0 : ((param - paramRange ().first) / L );
        if (order == 1) {
          pinocchio::difference <pinocchio::RnxSOnLieGroupMap>
            (robot_, end_, initial_, result);
          result /= L;
        } else if (order > 1) {
          result.setZero();
        }

        value_type t (u * curveLength_);
        value_type beta (curveLength_ / L);
        if (L == 0) beta = 1;
        value_type alpha (fabs(beta));
        if (forward_ == -1 && order%2 == 1) alpha *= -1;
        value_type c0 (initial_ [rzId_ + 0]), s0 (initial_ [rzId_ + 1]);
        value_type dx, dy, dtheta = 0;
        value_type c (cos (curvature_ * t)), s (sin (curvature_ * t));

        if (order <= 0) {
          std::ostringstream oss;
          oss << "order of derivative (" << order << ") should be positive.";
          throw std::runtime_error (oss.str ().c_str ());
        }
        if (order == 1) {
          dx = alpha * (c0 * c - s0 * s);
          dy = alpha * (c0 * s + s0 * c);
          dtheta = alpha * curvature_;
        } else {
          switch (order % 4) {
            case 2:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
            case 3:
              dx = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
            case 0:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
            case 1:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
              dy =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
          }
        }

        result [dxyId_ + 0] = dx;
        result [dxyId_ + 1] = dy;
        result [rzId_] = dtheta;
        // Express velocity in local frame
        Eigen::Matrix<value_type, 2, 2> R;
        R.col(0) << c0 * c - s0 * s, c0 * s + s0 * c;
        R.col(1) << - R(1,0), R(0,0);
        result.segment<2>(dxyId_) = R.transpose() * result.segment<2>(dxyId_);

        // Set wheel joint velocities
        for (std::vector<Wheels_t>::const_iterator w = wheels_.begin ();
             w < wheels_.end (); ++w) {
          result [w->j->rankInConfiguration ()] = 0;
        }
      }

      PathPtr_t ConstantCurvature::impl_extract
      (const interval_t& paramInterval) const throw (projection_error)
      {
        assert (!timeParameterization ());
        assert (timeRange ().second - timeRange ().first >= 0);
        value_type L (timeRange ().second - timeRange ().first);
        value_type tmin (paramInterval.first), tmax (paramInterval.second);
        value_type curveLength =0, pathLength = 0;
        if (L != 0) {
          curveLength = ((tmax - tmin)/L * curveLength_);
          pathLength = fabs (tmax - tmin)/L * length ();
        }
        Configuration_t init (robot_->configSize ());
        bool res (impl_compute (init, tmin)); assert (res);
        Configuration_t end (robot_->configSize ());
        res = impl_compute (end,tmax); assert (res);
        ConstantCurvaturePtr_t result (createCopy (weak_.lock ()));
        result->initial_ = init;
        result->end_ = end;
        result->curveLength_ = curveLength;
        result->timeRange (interval_t (0, pathLength));
        result->forward_ = curveLength > 0 ? 1 : -1;
        return result;
      }

      inline value_type meanBounds(const JointPtr_t& j, const size_type& i)
      {
        return (j->upperBound(i) + j->lowerBound(i))/2;
      }

      inline value_type saturate (const value_type& v, const JointPtr_t& j,
                                  const size_type& i)
      {
        return std::min(j->upperBound(i), std::max(j->lowerBound(i), v));
      }

      void ConstantCurvature::setWheelJoints
      (const JointPtr_t rz, const std::vector<JointPtr_t> wheels)
      {
        Transform3f zt (rz->currentTransformation ().inverse ());
        wheels_.resize(wheels.size());
        std::size_t rk = 0;
        if (curvature_ == 0) {
          for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
               _wheels != wheels.end(); ++_wheels) {
            wheels_[rk].j = *_wheels;
            wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
            ++rk;
          }
        } else {
          value_type rho (1./curvature_);
          for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
               _wheels != wheels.end(); ++_wheels) {
            wheels_[rk].j = *_wheels;
            wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
            const vector3_t wheelPos = zt.act
              (wheels_[rk].j->currentTransformation().translation());
            const value_type value (std::atan(wheelPos[0] /
                                              (rho - wheelPos[1])));
            wheels_[rk].value = saturate(meanBounds(wheels_[rk].j, 0) + value,
                                         *_wheels, 0);
            ++rk;
          }
        }
      }

    } // namespace steeringMethod
  } // namespace core
} // namespace hpp
<commit_msg>[ConstantCurvature] Fix computation of velocity.<commit_after>//
// Copyright (c) 2017 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/steering-method/constant-curvature.hh>

#include <pinocchio/spatial/se3.hpp>

#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup.hh>

namespace hpp {
  namespace core {
    namespace steeringMethod {

      ConstantCurvaturePtr_t ConstantCurvature::create
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
       const ConstraintSetPtr_t& constraints)
      {
        ConstantCurvature* ptr;
        if (constraints)
          ptr = new ConstantCurvature (
              robot, init, end, curveLength, pathLength,
              curvature, xyId, rzId, rz, wheels, constraints);
        else
          ptr = new ConstantCurvature (
              robot, init, end, curveLength, pathLength,
              curvature, xyId, rzId, rz, wheels);
        ConstantCurvaturePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      ConstantCurvaturePtr_t ConstantCurvature::createCopy
      (const ConstantCurvaturePtr_t& other)
      {
        ConstantCurvature* ptr (new ConstantCurvature (*other));
        ConstantCurvaturePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      ConstantCurvaturePtr_t ConstantCurvature::createCopy
      (const ConstantCurvaturePtr_t& other,
       const ConstraintSetPtr_t& constraints)
      {
	ConstantCurvature* ptr = new ConstantCurvature (*other, constraints);
	ConstantCurvaturePtr_t shPtr (ptr);
	ptr->init (shPtr);
	return shPtr;
      }

      /// Return a shared pointer to a copy of this
      PathPtr_t ConstantCurvature::copy () const
      {
	return createCopy (weak_.lock ());
      }

      std::ostream& ConstantCurvature::print (std::ostream &os) const
      {
        os << "-- ConstantCurvature" << std::endl;
        os << "from " << initial_.transpose () << std::endl;
        os << "  length: " << forward_ * paramLength() << std::endl;
        os << "  curvature: " << curvature_ << std::endl;
        return os;
      }

      ConstantCurvature::ConstantCurvature
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
       ConstraintSetPtr_t constraints) :
        Path (std::make_pair (0., fabs (pathLength)), robot->configSize (),
              robot->numberDof (), constraints), robot_ (robot),
        initial_ (init), end_ (end),
        curveLength_ (curveLength), curvature_ (curvature),
        xyId_ (xyId), rzId_ (rzId), forward_ (curveLength > 0 ? 1 : -1)
      {
        // Find rank of translation and rotation in velocity vectors
        // Hypothesis: degrees of freedom all belong to a planar joint or
        // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
        JointPtr_t joint (robot_->getJointAtConfigRank (xyId_));
        size_type offset (xyId_ - joint->rankInConfiguration ());
        dxyId_ = joint->rankInVelocity () + offset;
        joint = robot_->getJointAtConfigRank (rzId_);
        offset = rzId_ - joint->rankInConfiguration ();
        drzId_ = joint->rankInVelocity () + offset;
        setWheelJoints (rz, wheels);
        impl_compute (end_, paramRange ().second);
      }

      ConstantCurvature::ConstantCurvature
      (const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
       value_type curveLength, value_type pathLength, value_type curvature,
       size_type xyId, size_type rzId,
       const JointPtr_t rz, const std::vector<JointPtr_t> wheels) :
        Path (std::make_pair (0., fabs (pathLength)), robot->configSize (),
              robot->numberDof ()), robot_ (robot),
        initial_ (init), end_ (end), curveLength_ (curveLength),
        curvature_ (curvature), xyId_ (xyId),
        rzId_ (rzId), forward_ (curveLength > 0 ? 1 : -1)
      {
        // Find rank of translation and rotation in velocity vectors
        // Hypothesis: degrees of freedom all belong to a planar joint or
        // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
        JointPtr_t joint (robot_->getJointAtConfigRank (xyId_));
        size_type offset (xyId_ - joint->rankInConfiguration ());
        dxyId_ = joint->rankInVelocity () + offset;
        joint = robot_->getJointAtConfigRank (rzId_);
        offset = rzId_ - joint->rankInConfiguration ();
        drzId_ = joint->rankInVelocity () + offset;
        setWheelJoints (rz, wheels);
        impl_compute (end_, paramRange ().second);
      }

      ConstantCurvature::ConstantCurvature (const ConstantCurvature& other) :
        Path (other), robot_ (other.robot_), initial_ (other.initial_),
        end_ (other.end_), curveLength_ (other.curveLength_),
        curvature_ (other.curvature_),  xyId_ (other.xyId_),
        rzId_ (other.rzId_), dxyId_ (other.dxyId_), drzId_ (other.drzId_),
        forward_ (other.forward_), wheels_ (other.wheels_)
      {
      }

      ConstantCurvature::ConstantCurvature
      (const ConstantCurvature& other, const ConstraintSetPtr_t& constraints) :
        parent_t (other, constraints), robot_ (other.robot_),
        initial_ (other.initial_), end_ (other.end_),
        curveLength_ (other.curveLength_), curvature_ (other.curvature_),
        xyId_ (other.xyId_), rzId_ (other.rzId_), dxyId_ (other.dxyId_),
        drzId_ (other.drzId_), forward_ (other.forward_),
        wheels_ (other.wheels_)
      {
      }

      bool ConstantCurvature::impl_compute (ConfigurationOut_t result,
                                            value_type param) const
      {
        const value_type L = paramLength();
        // Does a linear interpolation on all the joints.
        const value_type u = (L == 0) ? 0 : ((param - paramRange ().first) / L );
        pinocchio::interpolate <pinocchio::RnxSOnLieGroupMap>(robot_, initial_, end_, u, result);

        value_type t (u * curveLength_);
        value_type x0 (initial_ [xyId_ + 0]), y0 (initial_ [xyId_ + 1]);
        value_type c0 (initial_ [rzId_ + 0]), s0 (initial_ [rzId_ + 1]);
        value_type x, y;
        value_type c (cos (curvature_ * t)), s (sin (curvature_ * t));

        if (curvature_ == 0) {
          x  = x0 + t * c0;
          y  = y0 + t * s0;
        } else {
          value_type r (1./curvature_);
          x = x0 + r * (s0 * (c - 1) + c0 * s);
          y = y0 + r * (c0 * (1 - c) + s0 * s);
        }
        result [xyId_ + 0] = x;
        result [xyId_ + 1] = y;
        result [rzId_ + 0] = c0 * c - s0 * s;
        result [rzId_ + 1] = c0 * s + s0 * c;

        // Set wheel joint positions
        for (std::vector<Wheels_t>::const_iterator w = wheels_.begin ();
             w < wheels_.end (); ++w) {
          result [w->j->rankInConfiguration ()] = w->value;
        }
        return true;
      }

      void ConstantCurvature::impl_derivative
      (vectorOut_t result, const value_type& param, size_type order) const
      {
        const value_type L = paramLength();
        // Does a linear interpolation on all the joints.
        const value_type u = (L == 0) ? 0 : ((param - paramRange ().first) / L );
        if (order == 1) {
          pinocchio::difference <pinocchio::RnxSOnLieGroupMap>
            (robot_, end_, initial_, result);
          result /= L;
        } else if (order > 1) {
          result.setZero();
        }

        value_type t (u * curveLength_);
        value_type beta (curveLength_ / L);
        if (L == 0) beta = 1;
        value_type alpha (fabs(beta));
        if (forward_ == -1 && order%2 == 1) alpha *= -1;
        value_type c0 (initial_ [rzId_ + 0]), s0 (initial_ [rzId_ + 1]);
        value_type dx, dy, dtheta = 0;
        value_type c (cos (curvature_ * t)), s (sin (curvature_ * t));

        if (order <= 0) {
          std::ostringstream oss;
          oss << "order of derivative (" << order << ") should be positive.";
          throw std::runtime_error (oss.str ().c_str ());
        }
        if (order == 1) {
          dx = alpha * (c0 * c - s0 * s);
          dy = alpha * (c0 * s + s0 * c);
          dtheta = alpha * curvature_;
        } else {
          switch (order % 4) {
            case 2:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
            case 3:
              dx = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
            case 0:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
              dy = -alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
            case 1:
              dx =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * c - s0 * s);
              dy =  alpha * pow (curvature_ * beta, (value_type) (order - 1)) *
                (c0 * s + s0 * c);
          }
        }

        result [dxyId_ + 0] = dx;
        result [dxyId_ + 1] = dy;
        result [drzId_] = dtheta;
        // Express velocity in local frame
        Eigen::Matrix<value_type, 2, 2> R;
        R.col(0) << c0 * c - s0 * s, c0 * s + s0 * c;
        R.col(1) << - R(1,0), R(0,0);
        result.segment<2>(dxyId_) = R.transpose() * result.segment<2>(dxyId_);

        // Set wheel joint velocities
        for (std::vector<Wheels_t>::const_iterator w = wheels_.begin ();
             w < wheels_.end (); ++w) {
          result [w->j->rankInVelocity ()] = 0;
        }
      }

      PathPtr_t ConstantCurvature::impl_extract
      (const interval_t& paramInterval) const throw (projection_error)
      {
        assert (!timeParameterization ());
        assert (timeRange ().second - timeRange ().first >= 0);
        value_type L (timeRange ().second - timeRange ().first);
        value_type tmin (paramInterval.first), tmax (paramInterval.second);
        value_type curveLength =0, pathLength = 0;
        if (L != 0) {
          curveLength = ((tmax - tmin)/L * curveLength_);
          pathLength = fabs (tmax - tmin)/L * length ();
        }
        Configuration_t init (robot_->configSize ());
        bool res (impl_compute (init, tmin)); assert (res);
        Configuration_t end (robot_->configSize ());
        res = impl_compute (end,tmax); assert (res);
        ConstantCurvaturePtr_t result (createCopy (weak_.lock ()));
        result->initial_ = init;
        result->end_ = end;
        result->curveLength_ = curveLength;
        result->timeRange (interval_t (0, pathLength));
        result->forward_ = curveLength > 0 ? 1 : -1;
        return result;
      }

      inline value_type meanBounds(const JointPtr_t& j, const size_type& i)
      {
        return (j->upperBound(i) + j->lowerBound(i))/2;
      }

      inline value_type saturate (const value_type& v, const JointPtr_t& j,
                                  const size_type& i)
      {
        return std::min(j->upperBound(i), std::max(j->lowerBound(i), v));
      }

      void ConstantCurvature::setWheelJoints
      (const JointPtr_t rz, const std::vector<JointPtr_t> wheels)
      {
        Transform3f zt (rz->currentTransformation ().inverse ());
        wheels_.resize(wheels.size());
        std::size_t rk = 0;
        if (curvature_ == 0) {
          for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
               _wheels != wheels.end(); ++_wheels) {
            wheels_[rk].j = *_wheels;
            wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
            ++rk;
          }
        } else {
          value_type rho (1./curvature_);
          for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
               _wheels != wheels.end(); ++_wheels) {
            wheels_[rk].j = *_wheels;
            wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
            const vector3_t wheelPos = zt.act
              (wheels_[rk].j->currentTransformation().translation());
            const value_type value (std::atan(wheelPos[0] /
                                              (rho - wheelPos[1])));
            wheels_[rk].value = saturate(meanBounds(wheels_[rk].j, 0) + value,
                                         *_wheels, 0);
            ++rk;
          }
        }
      }

    } // namespace steeringMethod
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>
#include <assert.h>
#include <iostream>

#include "gd.h"
#include "vw.h"

using namespace std;
using namespace LEARNER;

namespace SVRG {

#define W_INNER      0   // working "inner-loop" weights, updated per example
#define W_STABLE     1   // stable weights, updated per stage
#define W_STABLEGRAD 2   // gradient corresponding to stable weights

struct svrg
{
  int stage_size;               // Number of data passes per stage.
  size_t prev_pass;             // To detect that we're in a new pass.
  int stable_grad_count;        // Number of data points that
                                // contributed to the stable gradient
                                // calculation.

  // The VW process' global state.
  vw* all;
};

// Mimic GD::inline_predict but with offset for predicting with either
// stable versus inner weights.

template<int offset>
inline void vec_add(float& p, const float x, float& w) {
  float* ws = &w;
  p += x * ws[offset];
}

template<int offset>
inline float inline_predict(vw& all, example& ec)
{
  float acc = ec.l.simple.initial;
  GD::foreach_feature<float, vec_add<offset> >(all, ec, acc);
  return acc;
}

// -- Prediction, using inner vs. stable weights --

float predict_stable(const svrg& s, example& ec)
{
  return GD::finalize_prediction(s.all->sd, inline_predict<W_STABLE>(*s.all, ec));
}

void predict(svrg& s, base_learner&, example& ec)
{
  ec.partial_prediction = inline_predict<W_INNER>(*s.all, ec);
  ec.pred.scalar = GD::finalize_prediction(s.all->sd, ec.partial_prediction);
}

float gradient_scalar(const svrg& s, const example& ec, float pred)
{
  return s.all->loss->first_derivative(s.all->sd, pred, ec.l.simple.label)
    * ec.l.simple.weight;
}

// -- Updates, taking inner steps vs. accumulating a full gradient --

struct update {
  float g_scalar_stable;
  float g_scalar_inner;
  float eta;
  float norm;
};

inline void update_inner_feature(update& u, float x, float& w)
{
  float* ws = &w;
  w -= u.eta * ((u.g_scalar_inner - u.g_scalar_stable) * x + ws[W_STABLEGRAD] / u.norm);
}

inline void update_stable_feature(float& g_scalar, float x, float& w)
{
  float* ws = &w;
  ws[W_STABLEGRAD] += g_scalar * x;
}

void update_inner(const svrg& s, example& ec)
{
  update u;
  // |ec| already has prediction according to inner weights.
  u.g_scalar_inner = gradient_scalar(s, ec, ec.pred.scalar);
  u.g_scalar_stable = gradient_scalar(s, ec, predict_stable(s, ec));
  u.eta = s.all->eta;
  u.norm = (float) s.stable_grad_count;
  GD::foreach_feature<update, update_inner_feature>(*s.all, ec, u);
}

void update_stable(const svrg& s, example& ec)
{
  float g = gradient_scalar(s, ec, predict_stable(s, ec));
  GD::foreach_feature<float, update_stable_feature>(*s.all, ec, g);
}

void learn(svrg& s, base_learner& base, example& ec)
{
  assert(ec.in_use);

  predict(s, base, ec);

  const size_t pass = s.all->passes_complete;

  if (pass % (s.stage_size + 1) == 0) { // Compute exact gradient
    if (s.prev_pass != pass && !s.all->quiet) {
      cout << "svrg pass " << pass << ": committing stable point" << endl;
      for (uint32_t j = 0; j < VW::num_weights(*s.all); j++) {
        float w = VW::get_weight(*s.all, j, W_INNER);
        VW::set_weight(*s.all, j, W_STABLE, w);
        VW::set_weight(*s.all, j, W_STABLEGRAD, 0.f);
      }
      s.stable_grad_count = 0;
      cout << "svrg pass " << pass << ": computing exact gradient" << endl;
    }
    update_stable(s, ec);
    s.stable_grad_count++;
  } else {                      // Perform updates
    if (s.prev_pass != pass && !s.all->quiet) {
      cout << "svrg pass " << pass << ": taking steps" << endl;
    }
    update_inner(s, ec);
  }

  s.prev_pass = pass;
}

void save_load(svrg& s, io_buf& model_file, bool read, bool text)
{
  if (read) {
    initialize_regressor(*s.all);
  }

  if (model_file.files.size() > 0) {
    bool resume = s.all->save_resume;
    char buff[512];
    uint32_t text_len = sprintf(buff, ":%d\n", resume);
    bin_text_read_write_fixed(model_file, (char*) &resume, sizeof(resume), "",
                              read, buff, text_len, text);

    if (resume) {
      GD::save_load_online_state(*s.all, model_file, read, text);
    } else {
      GD::save_load_regressor(*s.all, model_file, read, text);
    }
  }
}

}

using namespace SVRG;

base_learner* svrg_setup(vw& all)
{
  if (missing_option(all, false, "svrg", "Streaming Stochastic Variance Reduced Gradient")) {
    return NULL;
  }
  new_options(all, "SVRG options")
    ("stage_size", po::value<int>()->default_value(1), "Number of passes per SVRG stage");
  add_options(all);

  svrg& s = calloc_or_die<svrg>();
  s.all = &all;
  s.stage_size = all.vm["stage_size"].as<int>();
  s.prev_pass = -1;
  s.stable_grad_count = 0;

  // Request more parameter storage (4 floats per feature)
  all.reg.stride_shift = 2;
  learner<svrg>& l = init_learner(&s, learn, 1 << all.reg.stride_shift);

  l.set_predict(predict);
  l.set_save_load(save_load);
  return make_base(l);
}

<commit_msg>vowpalwabbit/svrg.cc: fix type warning differently: preserve the `int` type of `pass`.<commit_after>
#include <assert.h>
#include <iostream>

#include "gd.h"
#include "vw.h"

using namespace std;
using namespace LEARNER;

namespace SVRG {

#define W_INNER      0   // working "inner-loop" weights, updated per example
#define W_STABLE     1   // stable weights, updated per stage
#define W_STABLEGRAD 2   // gradient corresponding to stable weights

struct svrg
{
  int stage_size;               // Number of data passes per stage.
  int prev_pass;                // To detect that we're in a new pass.
  int stable_grad_count;        // Number of data points that
                                // contributed to the stable gradient
                                // calculation.

  // The VW process' global state.
  vw* all;
};

// Mimic GD::inline_predict but with offset for predicting with either
// stable versus inner weights.

template<int offset>
inline void vec_add(float& p, const float x, float& w) {
  float* ws = &w;
  p += x * ws[offset];
}

template<int offset>
inline float inline_predict(vw& all, example& ec)
{
  float acc = ec.l.simple.initial;
  GD::foreach_feature<float, vec_add<offset> >(all, ec, acc);
  return acc;
}

// -- Prediction, using inner vs. stable weights --

float predict_stable(const svrg& s, example& ec)
{
  return GD::finalize_prediction(s.all->sd, inline_predict<W_STABLE>(*s.all, ec));
}

void predict(svrg& s, base_learner&, example& ec)
{
  ec.partial_prediction = inline_predict<W_INNER>(*s.all, ec);
  ec.pred.scalar = GD::finalize_prediction(s.all->sd, ec.partial_prediction);
}

float gradient_scalar(const svrg& s, const example& ec, float pred)
{
  return s.all->loss->first_derivative(s.all->sd, pred, ec.l.simple.label)
    * ec.l.simple.weight;
}

// -- Updates, taking inner steps vs. accumulating a full gradient --

struct update {
  float g_scalar_stable;
  float g_scalar_inner;
  float eta;
  float norm;
};

inline void update_inner_feature(update& u, float x, float& w)
{
  float* ws = &w;
  w -= u.eta * ((u.g_scalar_inner - u.g_scalar_stable) * x + ws[W_STABLEGRAD] / u.norm);
}

inline void update_stable_feature(float& g_scalar, float x, float& w)
{
  float* ws = &w;
  ws[W_STABLEGRAD] += g_scalar * x;
}

void update_inner(const svrg& s, example& ec)
{
  update u;
  // |ec| already has prediction according to inner weights.
  u.g_scalar_inner = gradient_scalar(s, ec, ec.pred.scalar);
  u.g_scalar_stable = gradient_scalar(s, ec, predict_stable(s, ec));
  u.eta = s.all->eta;
  u.norm = (float) s.stable_grad_count;
  GD::foreach_feature<update, update_inner_feature>(*s.all, ec, u);
}

void update_stable(const svrg& s, example& ec)
{
  float g = gradient_scalar(s, ec, predict_stable(s, ec));
  GD::foreach_feature<float, update_stable_feature>(*s.all, ec, g);
}

void learn(svrg& s, base_learner& base, example& ec)
{
  assert(ec.in_use);

  predict(s, base, ec);

  const int pass = (int) s.all->passes_complete;

  if (pass % (s.stage_size + 1) == 0) { // Compute exact gradient
    if (s.prev_pass != pass && !s.all->quiet) {
      cout << "svrg pass " << pass << ": committing stable point" << endl;
      for (uint32_t j = 0; j < VW::num_weights(*s.all); j++) {
        float w = VW::get_weight(*s.all, j, W_INNER);
        VW::set_weight(*s.all, j, W_STABLE, w);
        VW::set_weight(*s.all, j, W_STABLEGRAD, 0.f);
      }
      s.stable_grad_count = 0;
      cout << "svrg pass " << pass << ": computing exact gradient" << endl;
    }
    update_stable(s, ec);
    s.stable_grad_count++;
  } else {                      // Perform updates
    if (s.prev_pass != pass && !s.all->quiet) {
      cout << "svrg pass " << pass << ": taking steps" << endl;
    }
    update_inner(s, ec);
  }

  s.prev_pass = pass;
}

void save_load(svrg& s, io_buf& model_file, bool read, bool text)
{
  if (read) {
    initialize_regressor(*s.all);
  }

  if (model_file.files.size() > 0) {
    bool resume = s.all->save_resume;
    char buff[512];
    uint32_t text_len = sprintf(buff, ":%d\n", resume);
    bin_text_read_write_fixed(model_file, (char*) &resume, sizeof(resume), "",
                              read, buff, text_len, text);

    if (resume) {
      GD::save_load_online_state(*s.all, model_file, read, text);
    } else {
      GD::save_load_regressor(*s.all, model_file, read, text);
    }
  }
}

}

using namespace SVRG;

base_learner* svrg_setup(vw& all)
{
  if (missing_option(all, false, "svrg", "Streaming Stochastic Variance Reduced Gradient")) {
    return NULL;
  }
  new_options(all, "SVRG options")
    ("stage_size", po::value<int>()->default_value(1), "Number of passes per SVRG stage");
  add_options(all);

  svrg& s = calloc_or_die<svrg>();
  s.all = &all;
  s.stage_size = all.vm["stage_size"].as<int>();
  s.prev_pass = -1;
  s.stable_grad_count = 0;

  // Request more parameter storage (4 floats per feature)
  all.reg.stride_shift = 2;
  learner<svrg>& l = init_learner(&s, learn, 1 << all.reg.stride_shift);

  l.set_predict(predict);
  l.set_save_load(save_load);
  return make_base(l);
}

<|endoftext|>