<commit_before>#include <iostream>
#include <string>
#include "STRTK/strtk.hpp"
#include "SFML/System.hpp"
#include "SFML/Graphics/Sprite.hpp"
#include "SFML/Graphics/Text.hpp"
#include "SFML/Graphics.hpp"
#include "painter.h"

#define WINDOW_WIDTH 1024
#define WINDOW_HEIGHT 768

Painter::Painter(RenderNode *node)
{
    sf::RenderWindow window(sf::VideoMode(WINDOW_WIDTH, WINDOW_HEIGHT, 32), "OpenWeb");
    paintNode(node, &window);
}

//SFML does not automatically wrap text to fit the window. This is a hurriedly
//implemented, temporary solution, and will receive more attention in the future.
//Right now, I am considering adopting SFGUI as an additional graphics library
//to address this (and other) issues I have found with SFML.
std::string Painter::parseTextToLines(std::string textToParse, int windowBoundary)
{
    std::vector<std::string> wordVector;

    strtk::parse(textToParse, " ", wordVector);

    std::string parsedString;
    std::string temporaryString;
    sf::Text tempString;

    for (int i = 0; i < wordVector.size(); i++)
    {
        temporaryString = temporaryString + wordVector.at(i) + " ";
        tempString.setString(temporaryString);
        if (tempString.getLocalBounds().width < windowBoundary)
        {
            parsedString = parsedString + wordVector.at(i) + " ";
        }
        else
        {
            parsedString = parsedString + "\n" + wordVector.at(i) + " ";
            tempString.setString("");
            temporaryString.clear();
        }
    }

    return parsedString;
}

void Painter::paintNode(RenderNode *node, sf::RenderWindow *window)
{
    try
    {
        sf::Text text;

        text.setString(parseTextToLines(node->getText(), 2048));
        text.setCharacterSize(15);

        sf::Font font;
        if (!font.loadFromFile("fonts/LinLibertine_R.ttf"))
        {
            throw "Error: Could not load the font.";
        }

        text.setFont(font);

        while (window->isOpen())
        {
            window->clear();
            window->draw(text);
            window->display();

            sf::Event event;
            while (window->pollEvent(event))
            {
                if (sf::Event::Closed == event.type)
                {
                    window->close();
                }
            }
        }
    }

    catch (std::string error)
    {
        std::cout << error << std::endl;
    }
}
<commit_msg>Commit to merge changes.<commit_after>#include <iostream>
#include <string>
#include "STRTK/strtk.hpp"
#include "SFML/System.hpp"
#include "SFML/Graphics/Sprite.hpp"
#include "SFML/Graphics/Text.hpp"
#include "SFML/Graphics.hpp"
#include "painter.h"

#define WINDOW_WIDTH 1024
#define WINDOW_HEIGHT 768

Painter::Painter(RenderNode *node)
{
    sf::RenderWindow window(sf::VideoMode(WINDOW_WIDTH, WINDOW_HEIGHT, 32), "OpenWeb");
    paintNode(node, &window);
}

//SFML does not automatically wrap text to fit the window. This is a hurriedly
//implemented, temporary solution, and will receive more attention in the future.
//Right now, I am considering adopting SFGUI as an additional graphics library
//to address this (and other) issues I have found with SFML.
std::string Painter::parseTextToLines(std::string textToParse, int windowBoundary)
{
    std::vector<std::string> wordVector;

    strtk::parse(textToParse, " ", wordVector);

    std::string parsedString;
    std::string temporaryString;
    sf::Text tempString;

    for (int i = 0; i < wordVector.size(); i++)
    {
        temporaryString = temporaryString + wordVector.at(i) + " ";
        tempString.setString(temporaryString);
        if (tempString.getLocalBounds().width < windowBoundary)
        {
            parsedString = parsedString + wordVector.at(i) + " ";
        }
        else
        {
            parsedString = parsedString + "\n" + wordVector.at(i) + " ";
            tempString.setString("");
            temporaryString.clear();
        }
    }

    return parsedString;
}

void Painter::paintNode(RenderNode *node, sf::RenderWindow *window)
{
    try
    {
        sf::Text text;

        text.setString(parseTextToLines(node->getText(), WINDOW_WIDTH));
        text.setCharacterSize(15);

        sf::Font font;
        if (!font.loadFromFile("fonts/LinLibertine_R.ttf"))
        {
            throw "Error: Could not load the font.";
        }

        text.setFont(font);

        while (window->isOpen())
        {
            window->clear();
            window->draw(text);
            window->display();

            sf::Event event;
            while (window->pollEvent(event))
            {
                if (sf::Event::Closed == event.type)
                {
                    window->close();
                }
            }
        }
    }

    catch (std::string error)
    {
        std::cout << error << std::endl;
    }
}
<|endoftext|>


<commit_before>#include "application.hpp"

#include <boost/log/attributes.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/support/date_time.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include <boost/log/utility/setup/console.hpp>
#include <iostream>
#include <cstdlib>

#include "log.hpp"

namespace asio    = boost::asio;
namespace logging = boost::log;

namespace simplicity
{
	xcb_atom_t xcbAtomDeleteWindow;
	xcb_atom_t xcbAtomProtocols;

	bool check_xcb_connection(xcb_connection_t *pConnection);

	const string SimplicityApplication::ENV_VAR_DISPLAY_NAME = "DISPLAY";

	SimplicityApplication &SimplicityApplication::get_instance(void)
	{
		static SimplicityApplication singleton;

		return singleton;
	}

	SimplicityApplication::SimplicityApplication(void) :
		m_bRunning(true),
		m_pXConnection(nullptr),
		m_GlobalLogger(),
		m_IOService(),
		m_Signals(m_IOService, SIGINT, SIGTERM, SIGHUP),
		m_sDisplayName("")
	{
		m_Signals.async_wait(boost::bind(&SimplicityApplication::handler_sig, this, _1, _2));

		m_ServiceThread = boost::thread(boost::bind(&asio::io_service::run, &m_IOService));
	}

	SimplicityApplication::~SimplicityApplication(void)
	{
		global_log_trace << "Destroying application singleton";
	}

	logging::sources::severity_logger<logging::trivial::severity_level> &SimplicityApplication::get_global_logger(void)
	{
		return m_GlobalLogger;
	}

	bool SimplicityApplication::run(void)
	{
		if (!m_bRunning)
			return true;

		global_log_trace << "Starting main application loop";

		xcbAtomDeleteWindow = get_atom("WM_DELETE_WINDOW");
		xcbAtomProtocols    = get_atom("WM_PROTOCOLS");

		m_bRunning = true;
		while (m_bRunning)
		{
			xcb_generic_event_t *pEvent = xcb_poll_for_event(m_pXConnection);

			if (pEvent == nullptr)
				continue;

			// TODO: do something with the event

			delete pEvent; // TODO: necessary?
		}

		xcb_disconnect(m_pXConnection);
		m_IOService.stop();

		return false;
	}

	void SimplicityApplication::quit(void)
	{
		global_log_trace << "Ending main application loop";

		xcb_client_message_event_t xcbEvent = {
			.response_type = XCB_CLIENT_MESSAGE,
			.format        = 32,
			.sequence      = 0,
			.window        = m_pRootScreen->root,
			.type          = xcbAtomProtocols,
			.data          = {
				(uint8_t)xcbAtomDeleteWindow,
				XCB_CURRENT_TIME
			}
		};

		xcb_send_event(
			m_pXConnection,
			false,
			m_pRootScreen->root,
			XCB_EVENT_MASK_NO_EVENT,
			(char *)&xcbEvent
		);

		xcb_flush(m_pXConnection);

		m_bRunning = false;
	}

	string SimplicityApplication::get_display_name(void) const
	{
		return m_sDisplayName;
	}

	void SimplicityApplication::set_display_name(string sDisplay)
	{
		if (sDisplay.empty())
		{
			char *pEnvVar = std::getenv(SimplicityApplication::ENV_VAR_DISPLAY_NAME.c_str());
			if (pEnvVar)
				m_sDisplayName = pEnvVar;
			else
				m_sDisplayName = ":0.0";

			global_log_trace << "No display set. Defaulting to " << m_sDisplayName;
		}
		else
		{
			m_sDisplayName = sDisplay;

			global_log_trace << "Display set to " << sDisplay;
		}

		putenv((char *)(SimplicityApplication::ENV_VAR_DISPLAY_NAME + "=" + m_sDisplayName).c_str());
	}

	void SimplicityApplication::handler_sig(const boost::system::error_code &error, int nSignal)
	{
		global_log_trace << "Received OS signal: " << nSignal;

		switch (nSignal)
		{
		case SIGHUP:
			global_log_trace << "Received OS SIGHUP";
			handler_sig_hup(error, nSignal);
			break;
		case SIGINT:
			global_log_trace << "Received OS SIGINT";
			handler_sig_int(error, nSignal);
			return;
		case SIGTERM:
			global_log_trace << "Received OS SIGTERM";
			handler_sig_term(error, nSignal);
			return;
		default:
			global_log_debug << "Unhandled signal: " << nSignal;
		}

		m_Signals.async_wait(boost::bind(&SimplicityApplication::handler_sig, this, _1, _2));
	}

	xcb_atom_t SimplicityApplication::get_atom(string sAtomName)
	{
		xcb_intern_atom_cookie_t xcbCookie = xcb_intern_atom(
			m_pXConnection,
			0,
			sAtomName.length(),
			sAtomName.c_str()
		);

		xcb_intern_atom_reply_t *pxcbReply = xcb_intern_atom_reply(
			m_pXConnection,
			xcbCookie,
			NULL
		);

		if (pxcbReply == nullptr)
			return 0;

		xcb_atom_t xcbAtom = pxcbReply->atom;
		delete pxcbReply;

		return xcbAtom;
	}

	void SimplicityApplication::initialize(string sDisplay)
	{
		set_display_name(sDisplay);

		initialize_logging();
		initialize_x_connection();
	}

	void SimplicityApplication::initialize_logging(void)
	{
		logging::add_common_attributes();

		logging::add_console_log(
			std::cerr,
			logging::keywords::format = (
				logging::expressions::stream
					<< "(" << logging::expressions::format_date_time<boost::posix_time::ptime>("TimeStamp", "%Y-%m-%d %H:%M%S") << ") "
					<< "[" << logging::trivial::severity << "]: "
					<< logging::expressions::smessage
			),
			logging::keywords::auto_flush = true
		);
	}

	void SimplicityApplication::initialize_x_connection(void)
	{
		int nScreenNum = 0;
		m_pXConnection = xcb_connect(m_sDisplayName.c_str(), &nScreenNum);

		if (check_xcb_connection(m_pXConnection))
		{
			xcb_disconnect(m_pXConnection);
			m_bRunning = false;
			return;
		}

		xcb_screen_iterator_t iter = xcb_setup_roots_iterator(xcb_get_setup(m_pXConnection));
		for (int i = 0; i != nScreenNum; i++)
			xcb_screen_next(&iter);

		m_pRootScreen = iter.data;
		if (m_pRootScreen == nullptr)
		{
			global_log_error << "Could not get the current screen. Exiting";
			xcb_disconnect(m_pXConnection);
			m_bRunning = false;
		}

		global_log_debug << "Root screen dimensions: "
						 << m_pRootScreen->width_in_pixels
						 << "x"
						 << m_pRootScreen->height_in_pixels
						 << "Root window: "
						 << m_pRootScreen->root;
	}

	void SimplicityApplication::handler_sig_hup(const boost::system::error_code &error, int nSignal)
	{
		// TODO: reload config

		global_log_debug << "SIGHUP received";
	}

	void SimplicityApplication::handler_sig_int(const boost::system::error_code &error, int nSignal)
	{
		quit();
	}

	void SimplicityApplication::handler_sig_term(const boost::system::error_code &error, int nSignal)
	{
		quit();
	}

	bool check_xcb_connection(xcb_connection_t *pConnection)
	{
		switch (xcb_connection_has_error(pConnection))
		{
		case XCB_CONN_ERROR:
			global_log_error << "Socket, pipe, or stream error prevented connetion to X server";
			return true;
		case XCB_CONN_CLOSED_EXT_NOTSUPPORTED:
			global_log_error << "Extension not supported. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_MEM_INSUFFICIENT:
			global_log_error << "Not enough memory. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_REQ_LEN_EXCEED:
			global_log_error << "Exceeded request length. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_PARSE_ERR:
			global_log_error << "Could not parse display string. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_INVALID_SCREEN:
			global_log_error << "Could not connect to X server because an invalid screen was selected";
			return true;
		default:
			return false;
		}
	}
}

<commit_msg>Do not allow calling of quit if not running<commit_after>#include "application.hpp"

#include <boost/log/attributes.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/support/date_time.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include <boost/log/utility/setup/console.hpp>
#include <iostream>
#include <cstdlib>

#include "log.hpp"

namespace asio    = boost::asio;
namespace logging = boost::log;

namespace simplicity
{
	xcb_atom_t xcbAtomDeleteWindow;
	xcb_atom_t xcbAtomProtocols;

	bool check_xcb_connection(xcb_connection_t *pConnection);

	const string SimplicityApplication::ENV_VAR_DISPLAY_NAME = "DISPLAY";

	SimplicityApplication &SimplicityApplication::get_instance(void)
	{
		static SimplicityApplication singleton;

		return singleton;
	}

	SimplicityApplication::SimplicityApplication(void) :
		m_bRunning(true),
		m_pXConnection(nullptr),
		m_GlobalLogger(),
		m_IOService(),
		m_Signals(m_IOService, SIGINT, SIGTERM, SIGHUP),
		m_sDisplayName("")
	{
		m_Signals.async_wait(boost::bind(&SimplicityApplication::handler_sig, this, _1, _2));

		m_ServiceThread = boost::thread(boost::bind(&asio::io_service::run, &m_IOService));
	}

	SimplicityApplication::~SimplicityApplication(void)
	{
		global_log_trace << "Destroying application singleton";
	}

	logging::sources::severity_logger<logging::trivial::severity_level> &SimplicityApplication::get_global_logger(void)
	{
		return m_GlobalLogger;
	}

	bool SimplicityApplication::run(void)
	{
		if (!m_bRunning)
			return true;

		global_log_trace << "Starting main application loop";

		xcbAtomDeleteWindow = get_atom("WM_DELETE_WINDOW");
		xcbAtomProtocols    = get_atom("WM_PROTOCOLS");

		m_bRunning = true;
		while (m_bRunning)
		{
			xcb_generic_event_t *pEvent = xcb_poll_for_event(m_pXConnection);

			if (pEvent == nullptr)
				continue;

			// TODO: do something with the event

			delete pEvent; // TODO: necessary?
		}

		xcb_disconnect(m_pXConnection);
		m_IOService.stop();

		return false;
	}

	void SimplicityApplication::quit(void)
	{
		if (!m_bRunning)
		{
			global_log_error << "Quit called but the application is not running";
			return;
		}

		global_log_trace << "Ending main application loop";

		xcb_client_message_event_t xcbEvent = {
			.response_type = XCB_CLIENT_MESSAGE,
			.format        = 32,
			.sequence      = 0,
			.window        = m_pRootScreen->root,
			.type          = xcbAtomProtocols,
			.data          = {
				(uint8_t)xcbAtomDeleteWindow,
				XCB_CURRENT_TIME
			}
		};

		xcb_send_event(
			m_pXConnection,
			false,
			m_pRootScreen->root,
			XCB_EVENT_MASK_NO_EVENT,
			(char *)&xcbEvent
		);

		xcb_flush(m_pXConnection);

		m_bRunning = false;
	}

	string SimplicityApplication::get_display_name(void) const
	{
		return m_sDisplayName;
	}

	void SimplicityApplication::set_display_name(string sDisplay)
	{
		if (sDisplay.empty())
		{
			char *pEnvVar = std::getenv(SimplicityApplication::ENV_VAR_DISPLAY_NAME.c_str());
			if (pEnvVar)
				m_sDisplayName = pEnvVar;
			else
				m_sDisplayName = ":0.0";

			global_log_trace << "No display set. Defaulting to " << m_sDisplayName;
		}
		else
		{
			m_sDisplayName = sDisplay;

			global_log_trace << "Display set to " << sDisplay;
		}

		putenv((char *)(SimplicityApplication::ENV_VAR_DISPLAY_NAME + "=" + m_sDisplayName).c_str());
	}

	void SimplicityApplication::handler_sig(const boost::system::error_code &error, int nSignal)
	{
		global_log_trace << "Received OS signal: " << nSignal;

		switch (nSignal)
		{
		case SIGHUP:
			global_log_trace << "Received OS SIGHUP";
			handler_sig_hup(error, nSignal);
			break;
		case SIGINT:
			global_log_trace << "Received OS SIGINT";
			handler_sig_int(error, nSignal);
			return;
		case SIGTERM:
			global_log_trace << "Received OS SIGTERM";
			handler_sig_term(error, nSignal);
			return;
		default:
			global_log_debug << "Unhandled signal: " << nSignal;
		}

		m_Signals.async_wait(boost::bind(&SimplicityApplication::handler_sig, this, _1, _2));
	}

	xcb_atom_t SimplicityApplication::get_atom(string sAtomName)
	{
		xcb_intern_atom_cookie_t xcbCookie = xcb_intern_atom(
			m_pXConnection,
			0,
			sAtomName.length(),
			sAtomName.c_str()
		);

		xcb_intern_atom_reply_t *pxcbReply = xcb_intern_atom_reply(
			m_pXConnection,
			xcbCookie,
			NULL
		);

		if (pxcbReply == nullptr)
			return 0;

		xcb_atom_t xcbAtom = pxcbReply->atom;
		delete pxcbReply;

		return xcbAtom;
	}

	void SimplicityApplication::initialize(string sDisplay)
	{
		set_display_name(sDisplay);

		initialize_logging();
		initialize_x_connection();
	}

	void SimplicityApplication::initialize_logging(void)
	{
		logging::add_common_attributes();

		logging::add_console_log(
			std::cerr,
			logging::keywords::format = (
				logging::expressions::stream
					<< "(" << logging::expressions::format_date_time<boost::posix_time::ptime>("TimeStamp", "%Y-%m-%d %H:%M%S") << ") "
					<< "[" << logging::trivial::severity << "]: "
					<< logging::expressions::smessage
			),
			logging::keywords::auto_flush = true
		);
	}

	void SimplicityApplication::initialize_x_connection(void)
	{
		int nScreenNum = 0;
		m_pXConnection = xcb_connect(m_sDisplayName.c_str(), &nScreenNum);

		if (check_xcb_connection(m_pXConnection))
		{
			xcb_disconnect(m_pXConnection);
			m_bRunning = false;
			return;
		}

		xcb_screen_iterator_t iter = xcb_setup_roots_iterator(xcb_get_setup(m_pXConnection));
		for (int i = 0; i != nScreenNum; i++)
			xcb_screen_next(&iter);

		m_pRootScreen = iter.data;
		if (m_pRootScreen == nullptr)
		{
			global_log_error << "Could not get the current screen. Exiting";
			xcb_disconnect(m_pXConnection);
			m_bRunning = false;
		}

		global_log_debug << "Root screen dimensions: "
						 << m_pRootScreen->width_in_pixels
						 << "x"
						 << m_pRootScreen->height_in_pixels
						 << "Root window: "
						 << m_pRootScreen->root;
	}

	void SimplicityApplication::handler_sig_hup(const boost::system::error_code &error, int nSignal)
	{
		// TODO: reload config

		global_log_debug << "SIGHUP received";
	}

	void SimplicityApplication::handler_sig_int(const boost::system::error_code &error, int nSignal)
	{
		quit();
	}

	void SimplicityApplication::handler_sig_term(const boost::system::error_code &error, int nSignal)
	{
		quit();
	}

	bool check_xcb_connection(xcb_connection_t *pConnection)
	{
		switch (xcb_connection_has_error(pConnection))
		{
		case XCB_CONN_ERROR:
			global_log_error << "Socket, pipe, or stream error prevented connetion to X server";
			return true;
		case XCB_CONN_CLOSED_EXT_NOTSUPPORTED:
			global_log_error << "Extension not supported. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_MEM_INSUFFICIENT:
			global_log_error << "Not enough memory. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_REQ_LEN_EXCEED:
			global_log_error << "Exceeded request length. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_PARSE_ERR:
			global_log_error << "Could not parse display string. Could not connect to X server";
			return true;
		case XCB_CONN_CLOSED_INVALID_SCREEN:
			global_log_error << "Could not connect to X server because an invalid screen was selected";
			return true;
		default:
			return false;
		}
	}
}

<|endoftext|>


<commit_before>//
//  pfilter.cpp
//  EpiGenMCMC
//
//  Created by Lucy Li on 05/05/2016.
//  Copyright (c) 2016 Lucy Li, Imperial College London. All rights reserved.
//

#include "pfilter.h"
#include <iostream>

namespace EpiGenPfilter {
    double pfilter(Model & sim_model, Parameter & model_params, MCMCoptions & options, Particle &particles, Trajectory & output_traj, TimeSeriesData &epi_data, TreeData &tree_data, MultiTreeData &multitree_data) {
        double loglik = 0.0;
        int num_groups = options.num_groups;
        int num_particles = options.particles;
        int init_seed = options.seed;
        int total_dt = options.total_dt;
        double sim_dt = options.sim_dt;
        int total_steps = ceil((double)total_dt/(double)options.pfilter_every);
        int add_dt = 0;
        double ESS_threshold = options.pfilter_threshold*(double)num_particles;
        Likelihood likelihood_calc;
//        std::vector <Parameter> values;// (options.num_threads, model_params);
//        for (int i=0; i!=options.num_threads; ++i) values.push_back(model_params);
//        for (int i=0; i!=model_params.get_total_params(); ++i) values.push_back(model_params.get(i));
        std::vector <std::vector<double> > values(options.num_threads, std::vector<double>(model_params.get_total_params(), 0.0));
        for (int i=0; i!=options.num_threads; ++i) {
            for (int j=0; j!=model_params.get_total_params(); ++j) {
                values[i][j] = model_params.get(j);
            }
        }
//        printf("Size of values = %d\n",values.size());
        double reporting_rate = 1.0;
        if (model_params.param_exists("reporting")) {
            reporting_rate = model_params.get("reporting");
        }
        std::vector <std::string> param_names = model_params.get_names_vector();
        if (model_params.param_exists("time_before_data")) {
            add_dt = model_params.get("time_before_data");
        }
        if (options.save_traj) {
            if (add_dt > 0) {
                particles.start_particle_tracing(add_dt+total_dt, num_groups);
            }
            else if (add_dt < 0) {
                particles.start_particle_tracing(add_dt+total_dt, num_groups);
                total_steps = ceil((double)(total_dt+add_dt)/(double)options.pfilter_every);
            }
            else {
                particles.start_particle_tracing(total_dt, num_groups);
            }
        }
        std::vector <Model> models;
        for (int i=0; i<options.num_threads; ++i) {
            models.push_back(sim_model);
        }
        // Simulate model and calculate likelihood assuming no observed data
        if (model_params.param_exists("time_before_data")) {
            if (add_dt > 0) {
                omp_set_num_threads(options.num_threads);
//                std::vector <Trajectory *> curr_trajs;
//                for (int i=0; i!=num_particles; ++i) {
//                    curr_trajs.push_back(particles.get_traj(i));
//                }
#pragma omp parallel for shared(particles, values) schedule(static, 1)
                for (int tn=0; tn<options.num_threads; tn++) {
                    for (int i=tn; i<num_particles; i+=options.num_threads) {
                        // Adjust length of trajectory
                        particles.get_traj(i)->resize(add_dt, num_groups);
                        models[tn].simulate(values[tn], param_names, particles.get_traj(i), 0, add_dt, sim_dt, total_dt, options.rng[tn]);
                        if (options.which_likelihood<2) {
                            double w = likelihood_calc.binomial_lik(reporting_rate, particles.get_traj(i)->get_total_traj(), add_dt+total_dt, 0, add_dt, num_groups, false);
                            particles.set_weight(w, i, false);
                        }
                        if (options.save_traj) {
                            particles.save_traj_to_matrix(i, 0, add_dt);
                            particles.save_ancestry(i, 0, add_dt);
                        }
                    }
                }
            }
        }
        init_seed += num_particles;
        int t=0;
        int start_dt;
        int end_dt;
        for (t=0; t!=total_steps; ++t) {
//            std::vector<double> we(options.particles, 0.0), wg(options.particles, 0.0);
            start_dt = t*options.pfilter_every;
            end_dt = std::min(total_dt, (t+1)*options.pfilter_every);
            omp_set_num_threads(options.num_threads);
#pragma omp parallel for shared (particles, values) schedule(static,1)
            for (int tn=0; tn<options.num_threads; tn++) {
                for (int i=tn; i<num_particles; i+=options.num_threads) {
                    // Adjust length of trajectory
                    particles.get_traj(i)->resize(end_dt-start_dt, options.num_groups);
                    models[tn].simulate(values[tn], param_names, particles.get_traj(i), start_dt, end_dt, sim_dt, total_dt, options.rng[tn]);
                    double w = 1.0;
                    double temp = 0.0;
                    if (options.which_likelihood<2) {
                        double A = particles.get_traj(i)->get_total_traj();
                        temp = likelihood_calc.binomial_lik(reporting_rate, A, epi_data.get_data_ptr(0), add_dt+total_dt, start_dt, end_dt, add_dt, options.num_groups, false);
                        w *= temp;
//                        we[i] = log(temp);
                    }
                    if (options.which_likelihood != 1) {
                        temp = likelihood_calc.coalescent_lik(particles.get_traj(i)->get_traj_ptr(1, 0), particles.get_traj(i)->get_traj_ptr(2, 0),
                                                            tree_data.get_binomial_ptr(0), tree_data.get_interval_ptr(0), tree_data.get_ends_ptr(0),
                                                            start_dt, end_dt, add_dt, false);
                        w *= temp;
//                        wg[i] = log(temp);
                    }
                    particles.set_weight(w, i, true);
                    if (options.save_traj) {
                        particles.save_traj_to_matrix(i, start_dt+add_dt, end_dt+add_dt);
                        particles.save_ancestry(i, start_dt+add_dt, end_dt+add_dt);
                    }
                }
            }
//            std::cout << "Epi Weight: " << std::accumulate(we.begin(), we.end(), 0.0) << " Gen Weight: " << std::accumulate(wg.begin(), wg.end(), 0.0) << " Total: " << particles.get_total_weight() << std::endl;
            double curr_ESS = particles.get_ESS();
            if (curr_ESS < ESS_threshold) {
                double total_weight = particles.get_total_weight();
                if (total_weight == 0.0) {
                    loglik += -0.1*std::numeric_limits<double>::max();
//                    std::cout << std::accumulate(epi_data.get_data_ptr(0)+start_dt, epi_data.get_data_ptr(0)+end_dt, 0.0) << " : " << particles.get_traj(0)->get_traj(0) << std::endl;
                    std::cout << "stop time: " << end_dt << std::endl;
                    break;
                } else {
                    loglik += log(total_weight) - log(num_particles);
                }
                particles.resample(options.rng[0]);
            }
            else {
                particles.reset_parents();
            }
        }
        if (options.save_traj) {
            output_traj.resize((total_dt+add_dt), num_groups);
            //if (loglik > -0.1*std::numeric_limits<double>::max()) {
                particles.retrace_traj(output_traj, options.rng[0]);
            //}
        }
        for (int i=0; i!=num_particles; ++i) {
            particles.get_traj(i)->reset();
        }
        std::vector < std::vector<double> >().swap(values);
        return (loglik);
    }
}
<commit_msg>latest working version<commit_after>//
//  pfilter.cpp
//  EpiGenMCMC
//
//  Created by Lucy Li on 05/05/2016.
//  Copyright (c) 2016 Lucy Li, Imperial College London. All rights reserved.
//

#include "pfilter.h"
#include <iostream>

namespace EpiGenPfilter {
    double pfilter(Model & sim_model, Parameter & model_params, MCMCoptions & options, Particle &particles, Trajectory & output_traj, TimeSeriesData &epi_data, TreeData &tree_data, MultiTreeData &multitree_data) {
        double loglik = 0.0;
        int num_groups = options.num_groups;
        int num_particles = options.particles;
        int init_seed = options.seed;
        int total_dt = options.total_dt;
        double sim_dt = options.sim_dt;
        int total_steps = ceil((double)total_dt/(double)options.pfilter_every);
        int add_dt = 0;
        double ESS_threshold = options.pfilter_threshold*(double)num_particles;
        Likelihood likelihood_calc;
//        std::vector <Parameter> values;// (options.num_threads, model_params);
//        for (int i=0; i!=options.num_threads; ++i) values.push_back(model_params);
//        for (int i=0; i!=model_params.get_total_params(); ++i) values.push_back(model_params.get(i));
        std::vector <std::vector<double> > values(options.num_threads, std::vector<double>(model_params.get_total_params(), 0.0));
        for (int i=0; i!=options.num_threads; ++i) {
            for (int j=0; j!=model_params.get_total_params(); ++j) {
                values[i][j] = model_params.get(j);
            }
        }
//        printf("Size of values = %d\n",values.size());
        double reporting_rate = 1.0;
        if (model_params.param_exists("reporting")) {
            reporting_rate = model_params.get("reporting");
        }
        std::vector <std::string> param_names = model_params.get_names_vector();
        if (model_params.param_exists("time_before_data")) {
            add_dt = model_params.get("time_before_data");
        }
        if (options.save_traj) {
            if (add_dt > 0) {
                particles.start_particle_tracing(add_dt+total_dt, num_groups);
            }
            else if (add_dt < 0) {
                particles.start_particle_tracing(add_dt+total_dt, num_groups);
                total_steps = ceil((double)(total_dt+add_dt)/(double)options.pfilter_every);
            }
            else {
                particles.start_particle_tracing(total_dt, num_groups);
            }
        }
        std::vector <Model> models;
        for (int i=0; i<options.num_threads; ++i) {
            models.push_back(sim_model);
        }
        // Simulate model and calculate likelihood assuming no observed data
        if (model_params.param_exists("time_before_data")) {
            if (add_dt > 0) {
                omp_set_num_threads(options.num_threads);
//                std::vector <Trajectory *> curr_trajs;
//                for (int i=0; i!=num_particles; ++i) {
//                    curr_trajs.push_back(particles.get_traj(i));
//                }
#pragma omp parallel for shared(particles, values) schedule(static, 1)
                for (int tn=0; tn<options.num_threads; tn++) {
                    for (int i=tn; i<num_particles; i+=options.num_threads) {
                        // Adjust length of trajectory
                        particles.get_traj(i)->resize(add_dt, num_groups);
                        models[tn].simulate(values[tn], param_names, particles.get_traj(i), 0, add_dt, sim_dt, total_dt, options.rng[tn]);
                        if (options.which_likelihood<2) {
                            double w = likelihood_calc.binomial_lik(reporting_rate, particles.get_traj(i)->get_total_traj(), add_dt+total_dt, 0, add_dt, num_groups, false);
                            particles.set_weight(w, i, false);
                        }
                        if (options.save_traj) {
                            particles.save_traj_to_matrix(i, 0, add_dt);
                            particles.save_ancestry(i, 0, add_dt);
                        }
                    }
                }
            }
        }
        init_seed += num_particles;
        int t=0;
        int start_dt;
        int end_dt;
        for (t=0; t!=total_steps; ++t) {
//            std::vector<double> we(options.particles, 0.0), wg(options.particles, 0.0);
            start_dt = t*options.pfilter_every;
            end_dt = std::min(total_dt, (t+1)*options.pfilter_every);
            omp_set_num_threads(options.num_threads);
#pragma omp parallel for shared (particles, values) schedule(static,1)
            for (int tn=0; tn<options.num_threads; tn++) {
                for (int i=tn; i<num_particles; i+=options.num_threads) {
                    // Adjust length of trajectory
//                    if (tn==0) std::cout << i << ' ' << std::endl;
                    particles.get_traj(i)->resize(end_dt-start_dt, options.num_groups);
                    models[tn].simulate(values[tn], param_names, particles.get_traj(i), start_dt, end_dt, sim_dt, total_dt, options.rng[tn]);
                    double w = 1.0;
                    double temp = 0.0;
                    if (options.which_likelihood<2) {
                        double A = particles.get_traj(i)->get_total_traj();
                        temp = likelihood_calc.binomial_lik(reporting_rate, A, epi_data.get_data_ptr(0), add_dt+total_dt, start_dt, end_dt, add_dt, options.num_groups, false);
                        w *= temp;
//                        we[i] = log(temp);
                    }
                    if (options.which_likelihood != 1) {
                        temp = likelihood_calc.coalescent_lik(particles.get_traj(i)->get_traj_ptr(1, 0), particles.get_traj(i)->get_traj_ptr(2, 0),
                                                            tree_data.get_binomial_ptr(0), tree_data.get_interval_ptr(0), tree_data.get_ends_ptr(0),
                                                            start_dt, end_dt, add_dt, false);
                        w *= temp;
//                        wg[i] = log(temp);
                    }
                    particles.set_weight(w, i, true);
                    if (options.save_traj) {
                        particles.save_traj_to_matrix(i, start_dt+add_dt, end_dt+add_dt);
                        particles.save_ancestry(i, start_dt+add_dt, end_dt+add_dt);
                    }
                }
            }
//            std::cout << "Epi Weight: " << std::accumulate(we.begin(), we.end(), 0.0) << " Gen Weight: " << std::accumulate(wg.begin(), wg.end(), 0.0) << " Total: " << particles.get_total_weight() << std::endl;
            double curr_ESS = particles.get_ESS();
            if (curr_ESS < ESS_threshold) {
                double total_weight = particles.get_total_weight();
                if (total_weight == 0.0) {
                    loglik += -0.1*std::numeric_limits<double>::max();
//                    std::cout << std::accumulate(epi_data.get_data_ptr(0)+start_dt, epi_data.get_data_ptr(0)+end_dt, 0.0) << " : " << particles.get_traj(0)->get_traj(0) << std::endl;
                    std::cout << "stop time: " << end_dt << std::endl;
                    break;
                } else {
                    loglik += log(total_weight) - log(num_particles);
                }
                particles.resample(options.rng[0]);
            }
            else {
                particles.reset_parents();
            }
        }
        if (options.save_traj) {
            output_traj.resize((total_dt+add_dt), num_groups);
            //if (loglik > -0.1*std::numeric_limits<double>::max()) {
                particles.retrace_traj(output_traj, options.rng[0]);
            //}
        }
        for (int i=0; i!=num_particles; ++i) {
            particles.get_traj(i)->reset();
        }
        std::vector < std::vector<double> >().swap(values);
        return (loglik);
    }
}
<|endoftext|>


<commit_before>/* This file exists to provide some stat monitors for process statistics and the like. */

#include <vector>
#include <string.h>
#include <stdlib.h>
#include <sys/syscall.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sstream>
#include <iomanip>
#include <stdarg.h>

#include "arch/io/io_utils.hpp"
#include "perfmon.hpp"
#include "logger.hpp"
#include "utils.hpp"
#include "thread_local.hpp"

/* Class to represent and parse the contents of /proc/[pid]/stat */

struct proc_pid_stat_exc_t : public std::exception {
    explicit proc_pid_stat_exc_t(const char *format, ...) __attribute__ ((format (printf, 2, 3))) {
        char buffer[2000];
        va_list l;
        va_start(l, format);
        vsnprintf(buffer, sizeof(buffer), format, l);
        va_end(l);
        msg = buffer;
    }
    std::string msg;
    const char *what() const throw () {
        return msg.c_str();
    }
    ~proc_pid_stat_exc_t() throw () { }
};

struct proc_pid_stat_t {
    int pid;
    char name[500];
    char state;
    int ppid, pgrp, session, tty_nr, tpgid;
    unsigned int flags;
    unsigned long int minflt, cminflt, majflt, cmajflt, utime, stime;
    long int cutime, cstime, priority, nice, num_threads, itrealvalue;
    long long unsigned int starttime;
    long unsigned int vsize;
    long int rss;
    long unsigned int rsslim, startcode, endcode, startstack, kstkesp, kstkeip, signal, blocked,
        sigignore, sigcatch, wchan, nswap, cnswap;
    int exit_signal, processor;
    unsigned int rt_priority, policy;
    long long unsigned int delayacct_blkio_ticks;
    long unsigned int guest_time;
    long int cguest_time;

    static proc_pid_stat_t for_pid(pid_t pid) {
        char path[100];
        snprintf(path, sizeof(path), "/proc/%d/stat", pid);
        proc_pid_stat_t stat;
        stat.read_from_file(path);
        return stat;
    }

    static proc_pid_stat_t for_pid_and_tid(pid_t pid, pid_t tid) {
        char path[100];
        snprintf(path, sizeof(path), "/proc/%d/task/%d/stat", pid, tid);
        proc_pid_stat_t stat;
        stat.read_from_file(path);
        return stat;
    }

private:
    void read_from_file(char * path) {
        scoped_fd_t stat_file(open(path, O_RDONLY));
        if (stat_file.get() == INVALID_FD) {
            throw proc_pid_stat_exc_t("Could not open '%s': %s (errno = %d)", path, strerror(errno), errno);
        }

        char buffer[1000];
        int res = ::read(stat_file.get(), buffer, sizeof(buffer));
        if (res <= 0) {
            throw proc_pid_stat_exc_t("Could not read '%s': %s (errno = %d)", path, strerror(errno), errno);
        }

        buffer[res] = '\0';

        // TODO rewrite this mess to use something more safe and sane, e.g. iostreams
        const int items_to_parse = 44;
        int res2 = sscanf(buffer, "%d %s %c %d %d %d %d %d %u %lu %lu %lu %lu %lu %lu %ld %ld "
            "%ld %ld %ld %ld %llu %lu %ld %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %d "
            "%d %u %u %u %u %llu %lu %ld",
            &pid, name, &state, &ppid, &pgrp, &session, &tty_nr, &tpgid,
            &flags, &minflt, &cminflt, &majflt, &cmajflt, &utime, &stime,
            &cutime, &cstime, &priority, &nice, &num_threads, &itrealvalue,
            &starttime, &vsize, &rss, &rsslim, &startcode, &endcode, &startstack,
            &kstkesp, &kstkeip, &signal, &blocked, &sigignore, &sigcatch, &wchan,
            &nswap, &cnswap, &exit_signal, &processor, &rt_priority, &processor,
            &rt_priority, &policy, &delayacct_blkio_ticks, &guest_time, &cguest_time);
        if (res2 != items_to_parse) {
            throw proc_pid_stat_exc_t("Could not parse '%s': expected to parse %d items, parsed "
                "%d. Buffer contents: %s", path, items_to_parse, res2, buffer);
        }
    }
};

/* perfmon_system_t is used to monitor system stats that do not need to be polled. */

struct perfmon_system_t :
    public perfmon_t
{
    bool have_reported_error;
    perfmon_system_t() : perfmon_t(false), have_reported_error(false) { }

    void *begin_stats() {
        return NULL;
    }
    void visit_stats(void *) {
    }
    void end_stats(void *, perfmon_stats_t *dest) {
        put_timestamp(dest);
        (*dest)["version"] = std::string(RETHINKDB_VERSION);
        (*dest)["pid"] = strprintf("%d", getpid());

        proc_pid_stat_t pid_stat;
        try {
            pid_stat = proc_pid_stat_t::for_pid(getpid());
        } catch (proc_pid_stat_exc_t e) {
            if (!have_reported_error) {
                logWRN("Error in reporting system stats: %s (Further errors like this will "
                    "be suppressed.)\n", e.what());
                have_reported_error = true;
            }
            return;
        }

        (*dest)["memory_virtual[bytes]"] = strprintf("%lu", pid_stat.vsize);
        (*dest)["memory_real[bytes]"] = strprintf("%ld", pid_stat.rss * sysconf(_SC_PAGESIZE));
    }
    void put_timestamp(perfmon_stats_t *dest) {
        timespec uptime = get_uptime();
        std::stringstream uptime_str;
        uptime_str << uptime.tv_sec << '.' << std::setfill('0') << std::setw(6) << uptime.tv_nsec / 1000;

        (*dest)["uptime"] = uptime_str.str();
        (*dest)["timestamp"] = format_precise_time(get_absolute_time(uptime));
    }
} pm_system;

/* Some of the stats need to be polled periodically. Call this function periodically on each
thread to ensure that stats are up to date. It takes a void* so that it can be called as a timer
callback. */

perfmon_sampler_t
    pm_cpu_user("cpu_user", secs_to_ticks(5)),
    pm_cpu_system("cpu_system", secs_to_ticks(5)),
    pm_cpu_combined("cpu_combined", secs_to_ticks(5)),
    pm_memory_faults("memory_faults", secs_to_ticks(5));

TLS(proc_pid_stat_t, last_stats);
TLS_with_init(ticks_t, last_ticks, 0);
TLS_with_init(bool, have_reported_stats_error, false);

void poll_system_stats(void *) {

    proc_pid_stat_t current_stats;
    try {
        current_stats = proc_pid_stat_t::for_pid_and_tid(getpid(), syscall(SYS_gettid));
    } catch (proc_pid_stat_exc_t e) {
        if (!TLS_get_have_reported_stats_error()) {
            logWRN("Error in reporting per-thread stats: %s (Further errors like this will "
                "be suppressed.)\n", e.what());
            TLS_set_have_reported_stats_error(true);
        }
    }
    ticks_t current_ticks = get_ticks();

    if (TLS_get_last_ticks() == 0) {
        TLS_set_last_stats(current_stats);
        TLS_set_last_ticks(current_ticks);
    } else if (current_ticks > TLS_get_last_ticks() + secs_to_ticks(1)) {
        double realtime_elapsed = ticks_to_secs(current_ticks - TLS_get_last_ticks()) * sysconf(_SC_CLK_TCK);
        pm_cpu_user.record((current_stats.utime - TLS_get_last_stats().utime) / realtime_elapsed);
        pm_cpu_system.record((current_stats.stime - TLS_get_last_stats().stime) / realtime_elapsed);
        pm_cpu_combined.record(
            (current_stats.utime - TLS_get_last_stats().utime +
             current_stats.stime - TLS_get_last_stats().stime) /
             realtime_elapsed);
        pm_memory_faults.record((current_stats.majflt - TLS_get_last_stats().majflt) / realtime_elapsed);

        TLS_set_last_stats(current_stats);
        TLS_set_last_ticks(current_ticks);
    }
}

<commit_msg>Fixed a bug in perfmon_system /proc/stat parsing, we had duplicated the processor and rt_policy arguments by accident.<commit_after>/* This file exists to provide some stat monitors for process statistics and the like. */

#include <vector>
#include <string.h>
#include <stdlib.h>
#include <sys/syscall.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sstream>
#include <iomanip>
#include <stdarg.h>

#include "arch/io/io_utils.hpp"
#include "perfmon.hpp"
#include "logger.hpp"
#include "utils.hpp"
#include "thread_local.hpp"

/* Class to represent and parse the contents of /proc/[pid]/stat */

struct proc_pid_stat_exc_t : public std::exception {
    explicit proc_pid_stat_exc_t(const char *format, ...) __attribute__ ((format (printf, 2, 3))) {
        char buffer[2000];
        va_list l;
        va_start(l, format);
        vsnprintf(buffer, sizeof(buffer), format, l);
        va_end(l);
        msg = buffer;
    }
    std::string msg;
    const char *what() const throw () {
        return msg.c_str();
    }
    ~proc_pid_stat_exc_t() throw () { }
};

struct proc_pid_stat_t {
    int pid;
    char name[500];
    char state;
    int ppid, pgrp, session, tty_nr, tpgid;
    unsigned int flags;
    unsigned long int minflt, cminflt, majflt, cmajflt, utime, stime;
    long int cutime, cstime, priority, nice, num_threads, itrealvalue;
    long long unsigned int starttime;
    long unsigned int vsize;
    long int rss;
    long unsigned int rsslim, startcode, endcode, startstack, kstkesp, kstkeip, signal, blocked,
        sigignore, sigcatch, wchan, nswap, cnswap;
    int exit_signal, processor;
    unsigned int rt_priority, policy;
    long long unsigned int delayacct_blkio_ticks;
    long unsigned int guest_time;
    long int cguest_time;

    static proc_pid_stat_t for_pid(pid_t pid) {
        char path[100];
        snprintf(path, sizeof(path), "/proc/%d/stat", pid);
        proc_pid_stat_t stat;
        stat.read_from_file(path);
        return stat;
    }

    static proc_pid_stat_t for_pid_and_tid(pid_t pid, pid_t tid) {
        char path[100];
        snprintf(path, sizeof(path), "/proc/%d/task/%d/stat", pid, tid);
        proc_pid_stat_t stat;
        stat.read_from_file(path);
        return stat;
    }

private:
    void read_from_file(char * path) {
        scoped_fd_t stat_file(open(path, O_RDONLY));
        if (stat_file.get() == INVALID_FD) {
            throw proc_pid_stat_exc_t("Could not open '%s': %s (errno = %d)", path, strerror(errno), errno);
        }

        char buffer[1000];
        int res = ::read(stat_file.get(), buffer, sizeof(buffer));
        if (res <= 0) {
            throw proc_pid_stat_exc_t("Could not read '%s': %s (errno = %d)", path, strerror(errno), errno);
        }

        buffer[res] = '\0';

        // TODO rewrite this mess to use something more safe and sane, e.g. iostreams
        const int items_to_parse = 44;
        int res2 = sscanf(buffer, "%d %s %c %d %d %d %d %d %u %lu %lu %lu %lu %lu %lu %ld %ld "
            "%ld %ld %ld %ld %llu %lu %ld %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %d "
            "%d %u %u %llu %lu %ld",
            &pid, name, &state, &ppid, &pgrp, &session, &tty_nr, &tpgid,
            &flags, &minflt, &cminflt, &majflt, &cmajflt, &utime, &stime,
            &cutime, &cstime, &priority, &nice, &num_threads, &itrealvalue,
            &starttime, &vsize, &rss, &rsslim, &startcode, &endcode, &startstack,
            &kstkesp, &kstkeip, &signal, &blocked, &sigignore, &sigcatch, &wchan,
            &nswap, &cnswap, &exit_signal, &processor, &rt_priority, &policy,
            &delayacct_blkio_ticks, &guest_time, &cguest_time);
        if (res2 != items_to_parse) {
            throw proc_pid_stat_exc_t("Could not parse '%s': expected to parse %d items, parsed "
                "%d. Buffer contents: %s", path, items_to_parse, res2, buffer);
        }
    }
};

/* perfmon_system_t is used to monitor system stats that do not need to be polled. */

struct perfmon_system_t :
    public perfmon_t
{
    bool have_reported_error;
    perfmon_system_t() : perfmon_t(false), have_reported_error(false) { }

    void *begin_stats() {
        return NULL;
    }
    void visit_stats(void *) {
    }
    void end_stats(void *, perfmon_stats_t *dest) {
        put_timestamp(dest);
        (*dest)["version"] = std::string(RETHINKDB_VERSION);
        (*dest)["pid"] = strprintf("%d", getpid());

        proc_pid_stat_t pid_stat;
        try {
            pid_stat = proc_pid_stat_t::for_pid(getpid());
        } catch (proc_pid_stat_exc_t e) {
            if (!have_reported_error) {
                logWRN("Error in reporting system stats: %s (Further errors like this will "
                    "be suppressed.)\n", e.what());
                have_reported_error = true;
            }
            return;
        }

        (*dest)["memory_virtual[bytes]"] = strprintf("%lu", pid_stat.vsize);
        (*dest)["memory_real[bytes]"] = strprintf("%ld", pid_stat.rss * sysconf(_SC_PAGESIZE));
    }
    void put_timestamp(perfmon_stats_t *dest) {
        timespec uptime = get_uptime();
        std::stringstream uptime_str;
        uptime_str << uptime.tv_sec << '.' << std::setfill('0') << std::setw(6) << uptime.tv_nsec / 1000;

        (*dest)["uptime"] = uptime_str.str();
        (*dest)["timestamp"] = format_precise_time(get_absolute_time(uptime));
    }
} pm_system;

/* Some of the stats need to be polled periodically. Call this function periodically on each
thread to ensure that stats are up to date. It takes a void* so that it can be called as a timer
callback. */

perfmon_sampler_t
    pm_cpu_user("cpu_user", secs_to_ticks(5)),
    pm_cpu_system("cpu_system", secs_to_ticks(5)),
    pm_cpu_combined("cpu_combined", secs_to_ticks(5)),
    pm_memory_faults("memory_faults", secs_to_ticks(5));

TLS(proc_pid_stat_t, last_stats);
TLS_with_init(ticks_t, last_ticks, 0);
TLS_with_init(bool, have_reported_stats_error, false);

void poll_system_stats(void *) {

    proc_pid_stat_t current_stats;
    try {
        current_stats = proc_pid_stat_t::for_pid_and_tid(getpid(), syscall(SYS_gettid));
    } catch (proc_pid_stat_exc_t e) {
        if (!TLS_get_have_reported_stats_error()) {
            logWRN("Error in reporting per-thread stats: %s (Further errors like this will "
                "be suppressed.)\n", e.what());
            TLS_set_have_reported_stats_error(true);
        }
    }
    ticks_t current_ticks = get_ticks();

    if (TLS_get_last_ticks() == 0) {
        TLS_set_last_stats(current_stats);
        TLS_set_last_ticks(current_ticks);
    } else if (current_ticks > TLS_get_last_ticks() + secs_to_ticks(1)) {
        double realtime_elapsed = ticks_to_secs(current_ticks - TLS_get_last_ticks()) * sysconf(_SC_CLK_TCK);
        pm_cpu_user.record((current_stats.utime - TLS_get_last_stats().utime) / realtime_elapsed);
        pm_cpu_system.record((current_stats.stime - TLS_get_last_stats().stime) / realtime_elapsed);
        pm_cpu_combined.record(
            (current_stats.utime - TLS_get_last_stats().utime +
             current_stats.stime - TLS_get_last_stats().stime) /
             realtime_elapsed);
        pm_memory_faults.record((current_stats.majflt - TLS_get_last_stats().majflt) / realtime_elapsed);

        TLS_set_last_stats(current_stats);
        TLS_set_last_ticks(current_ticks);
    }
}

<|endoftext|>


<commit_before>#include <cassert>
#include <cstdio>
#include <cstdlib>

#include <GL/glew.h>
#include <SDL2/SDL.h>

#include "FileUtil.h"

#define WINDOW_SIZE 640
#define ARRAY_COUNT(array) (sizeof(array) / sizeof(array[0]))

struct ShaderProgramSource {
    GLenum shaderType;
    const char* sourceFilename;
};

void printShaderInfoLog(GLuint shaderHandle, const char* name) {
    GLint logLength;
    glGetShaderiv(shaderHandle, GL_INFO_LOG_LENGTH, &logLength);
    if (logLength > 0) {
        char* logBuffer = new char[logLength];
        GLsizei bytesCopied;
        glGetShaderInfoLog(shaderHandle, logLength, &bytesCopied, logBuffer);
        (void)fprintf(stderr, "Shader '%s' info log:\n%s\n", name, logBuffer);
        delete [] logBuffer;
    }
}

GLuint compileShader(GLenum shaderType, const char* filename) {
    GLuint shaderHandle = glCreateShader(shaderType);
    if (shaderHandle != 0) {
        const GLchar* shaderCode = FileUtil::loadFileAsString(filename);
        glShaderSource(shaderHandle, 1, &shaderCode, NULL);
        delete [] shaderCode;
        glCompileShader(shaderHandle);

        GLint result;
        glGetShaderiv(shaderHandle, GL_COMPILE_STATUS, &result);
        if (result == GL_TRUE) {
            (void)printf("Shader '%s' compilation was successful!\n", filename);
            // Print warnings, if available
            printShaderInfoLog(shaderHandle, filename);
        } else {
            (void)fprintf(stderr, "Shader compilation failed for '%s'!\n", filename);
            printShaderInfoLog(shaderHandle, filename);
            glDeleteShader(shaderHandle);
            shaderHandle = 0;
        }
    } else {
        (void)fprintf(stderr, "Error creating shader for '%s'.\n", filename);
    }
    return shaderHandle;
}

void printProgramInfoLog(GLuint programHandle) {
    GLint logLength;
    glGetProgramiv(programHandle, GL_INFO_LOG_LENGTH, &logLength);
    if (logLength > 0) {
        char* logBuffer = new char[logLength];
        GLsizei bytesCopied;
        glGetProgramInfoLog(programHandle, logLength, &bytesCopied, logBuffer);
        (void)fprintf(stderr, "Program info log:\n%s\n", logBuffer);
        delete [] logBuffer;
    }
}

GLuint linkShaderProgram(const ShaderProgramSource* sources, size_t numSources) {
    assert(numSources != 0);
    GLuint programHandle = glCreateProgram();
    if (programHandle != 0) {
        for (int i = 0; i < numSources; ++i) {
            GLuint shaderHandle = compileShader(sources[i].shaderType, sources[i].sourceFilename);
            if (shaderHandle != 0) {
                glAttachShader(programHandle, shaderHandle);
                glDeleteShader(shaderHandle);
            }
        }
        glLinkProgram(programHandle);

        GLint linkStatus;
        glGetProgramiv(programHandle, GL_LINK_STATUS, &linkStatus);
        if (linkStatus == GL_TRUE) {
            (void)printf("Shader link was successful!\n");
            printProgramInfoLog(programHandle);
        } else {
            (void)fprintf(stderr, "Shader link failed!\n");
            printProgramInfoLog(programHandle);
            glDeleteProgram(programHandle);
            programHandle = 0;
        }
    } else {
        (void)fprintf(stderr, "Error creating shader program handle.\n");
        exit(EXIT_FAILURE);
    }

    return programHandle;
}

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

    int retval = SDL_Init(SDL_INIT_VIDEO);
    if (retval != 0) {
        (void)fprintf(stderr, "SDL_Init() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 4);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 0);
    SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
    SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);

    SDL_ClearError();
    SDL_Window* mainWindow = SDL_CreateWindow("Compiling a Shader",
                                              SDL_WINDOWPOS_CENTERED,
                                              SDL_WINDOWPOS_CENTERED,
                                              WINDOW_SIZE,
                                              WINDOW_SIZE,
                                              SDL_WINDOW_OPENGL | SDL_WINDOW_HIDDEN);
    if (mainWindow == NULL) {
        (void)fprintf(stderr, "SDL_CreateWindow() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    SDL_GLContext mainContext = SDL_GL_CreateContext(mainWindow);
    if (mainContext == NULL) {
        (void)fprintf(stderr, "SDL_GL_CreateContext() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    GLenum err = glewInit();
    if (err != GLEW_OK) {
        (void)fprintf(stderr, "Error Initializing GLEW: %s\n", glewGetErrorString(err));
        exit(EXIT_FAILURE);
    }

    ShaderProgramSource programSources[] = {{GL_VERTEX_SHADER, "basic.vert"},
                                            {GL_FRAGMENT_SHADER, "basic.frag"}};
    GLuint programHandle = linkShaderProgram(programSources, ARRAY_COUNT(programSources));
    if (programHandle != 0) {
        glUseProgram(programHandle);
    }

    SDL_GL_DeleteContext(mainContext);
    SDL_DestroyWindow(mainWindow);
    SDL_Quit();

    return 0;
}
<commit_msg>Typo.<commit_after>#include <cassert>
#include <cstdio>
#include <cstdlib>

#include <GL/glew.h>
#include <SDL2/SDL.h>

#include "FileUtil.h"

#define WINDOW_SIZE 640
#define ARRAY_COUNT(array) (sizeof(array) / sizeof(array[0]))

struct ShaderProgramSource {
    GLenum shaderType;
    const char* sourceFilename;
};

void printShaderInfoLog(GLuint shaderHandle, const char* name) {
    GLint logLength;
    glGetShaderiv(shaderHandle, GL_INFO_LOG_LENGTH, &logLength);
    if (logLength > 0) {
        char* logBuffer = new char[logLength];
        GLsizei bytesCopied;
        glGetShaderInfoLog(shaderHandle, logLength, &bytesCopied, logBuffer);
        (void)fprintf(stderr, "Shader '%s' info log:\n%s\n", name, logBuffer);
        delete [] logBuffer;
    }
}

GLuint compileShader(GLenum shaderType, const char* filename) {
    GLuint shaderHandle = glCreateShader(shaderType);
    if (shaderHandle != 0) {
        const GLchar* shaderCode = FileUtil::loadFileAsString(filename);
        glShaderSource(shaderHandle, 1, &shaderCode, NULL);
        delete [] shaderCode;
        glCompileShader(shaderHandle);

        GLint result;
        glGetShaderiv(shaderHandle, GL_COMPILE_STATUS, &result);
        if (result == GL_TRUE) {
            (void)printf("Shader '%s' compilation was successful!\n", filename);
            // Print warnings, if available
            printShaderInfoLog(shaderHandle, filename);
        } else {
            (void)fprintf(stderr, "Shader compilation failed for '%s'!\n", filename);
            printShaderInfoLog(shaderHandle, filename);
            glDeleteShader(shaderHandle);
            shaderHandle = 0;
        }
    } else {
        (void)fprintf(stderr, "Error creating shader for '%s'.\n", filename);
    }
    return shaderHandle;
}

void printProgramInfoLog(GLuint programHandle) {
    GLint logLength;
    glGetProgramiv(programHandle, GL_INFO_LOG_LENGTH, &logLength);
    if (logLength > 0) {
        char* logBuffer = new char[logLength];
        GLsizei bytesCopied;
        glGetProgramInfoLog(programHandle, logLength, &bytesCopied, logBuffer);
        (void)fprintf(stderr, "Program info log:\n%s\n", logBuffer);
        delete [] logBuffer;
    }
}

GLuint linkShaderProgram(const ShaderProgramSource* sources, size_t numSources) {
    assert(numSources != 0);
    GLuint programHandle = glCreateProgram();
    if (programHandle != 0) {
        for (int i = 0; i < numSources; ++i) {
            GLuint shaderHandle = compileShader(sources[i].shaderType, sources[i].sourceFilename);
            if (shaderHandle != 0) {
                glAttachShader(programHandle, shaderHandle);
                glDeleteShader(shaderHandle);
            }
        }
        glLinkProgram(programHandle);

        GLint linkStatus;
        glGetProgramiv(programHandle, GL_LINK_STATUS, &linkStatus);
        if (linkStatus == GL_TRUE) {
            (void)printf("Shader link was successful!\n");
            printProgramInfoLog(programHandle);
        } else {
            (void)fprintf(stderr, "Shader link failed!\n");
            printProgramInfoLog(programHandle);
            glDeleteProgram(programHandle);
            programHandle = 0;
        }
    } else {
        (void)fprintf(stderr, "Error creating shader program handle.\n");
        exit(EXIT_FAILURE);
    }

    return programHandle;
}

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

    int retval = SDL_Init(SDL_INIT_VIDEO);
    if (retval != 0) {
        (void)fprintf(stderr, "SDL_Init() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 4);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 0);
    SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
    SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);

    SDL_ClearError();
    SDL_Window* mainWindow = SDL_CreateWindow("Linking a Shader",
                                              SDL_WINDOWPOS_CENTERED,
                                              SDL_WINDOWPOS_CENTERED,
                                              WINDOW_SIZE,
                                              WINDOW_SIZE,
                                              SDL_WINDOW_OPENGL | SDL_WINDOW_HIDDEN);
    if (mainWindow == NULL) {
        (void)fprintf(stderr, "SDL_CreateWindow() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    SDL_GLContext mainContext = SDL_GL_CreateContext(mainWindow);
    if (mainContext == NULL) {
        (void)fprintf(stderr, "SDL_GL_CreateContext() failed: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    GLenum err = glewInit();
    if (err != GLEW_OK) {
        (void)fprintf(stderr, "Error Initializing GLEW: %s\n", glewGetErrorString(err));
        exit(EXIT_FAILURE);
    }

    ShaderProgramSource programSources[] = {{GL_VERTEX_SHADER, "basic.vert"},
                                            {GL_FRAGMENT_SHADER, "basic.frag"}};
    GLuint programHandle = linkShaderProgram(programSources, ARRAY_COUNT(programSources));
    if (programHandle != 0) {
        glUseProgram(programHandle);
    }

    SDL_GL_DeleteContext(mainContext);
    SDL_DestroyWindow(mainWindow);
    SDL_Quit();

    return 0;
}
<|endoftext|>


<commit_before>// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <asp/Core/Nvm.h>
#include <vw/Core/Exception.h>
#include <vw/Core/Log.h>
#include <vw/FileIO/FileUtils.h>

#include <fstream>
#include <iostream>

namespace asp {

// A wrapper to carry fewer things around
void ReadNVM(std::string const& input_filename, nvmData & nvm) {
  ReadNVM(input_filename,
          &nvm.cid_to_keypoint_map,
          &nvm.cid_to_filename,
          &nvm.pid_to_cid_fid,
          &nvm.pid_to_xyz,
          &nvm.cid_to_cam_t_global);
}

// Reads the NVM control network format.
void ReadNVM(std::string const& input_filename,
             std::vector<Eigen::Matrix2Xd> * cid_to_keypoint_map,
             std::vector<std::string> * cid_to_filename,
             std::vector<std::map<int, int>> * pid_to_cid_fid,
             std::vector<Eigen::Vector3d> * pid_to_xyz,
             std::vector<Eigen::Affine3d> * cid_to_cam_t_global) {

  std::ifstream f(input_filename, std::ios::in);
  std::string token;
  std::getline(f, token);

  // Assert that we start with our NVM token
  if (token.compare(0, 6, "NVM_V3") != 0) {
    vw::vw_throw(vw::ArgumentErr() << "File doesn't start with NVM token.");
  }

  // Read the number of cameras
  ptrdiff_t number_of_cid;
  f >> number_of_cid;
  if (number_of_cid < 1) {
    vw::vw_throw(vw::ArgumentErr() << "NVM file is missing cameras.");
  }

  // Resize all our structures to support the number of cameras we now expect
  cid_to_keypoint_map->resize(number_of_cid);
  cid_to_filename->resize(number_of_cid);
  cid_to_cam_t_global->resize(number_of_cid);
  for (ptrdiff_t cid = 0; cid < number_of_cid; cid++) {
    // Clear keypoints from map. We'll read these in shortly
    cid_to_keypoint_map->at(cid).resize(Eigen::NoChange_t(), 2);

    // Read the line that contains camera information
    double focal, dist1, dist2;
    Eigen::Quaterniond q;
    Eigen::Vector3d c;
    f >> token >> focal;
    f >> q.w() >> q.x() >> q.y() >> q.z();
    f >> c[0] >> c[1] >> c[2] >> dist1 >> dist2;
    cid_to_filename->at(cid) = token;

    // Solve for t, which is part of the affine transform
    Eigen::Matrix3d r = q.matrix();
    cid_to_cam_t_global->at(cid).linear() = r;
    cid_to_cam_t_global->at(cid).translation() = -r * c;
  }

  // Read the number of points
  ptrdiff_t number_of_pid;
  f >> number_of_pid;
  if (number_of_pid < 1)
    vw::vw_throw(vw::ArgumentErr() << "The NVM file has no triangulated points.");

  // Read the point
  pid_to_cid_fid->resize(number_of_pid);
  pid_to_xyz->resize(number_of_pid);
  Eigen::Vector3d xyz;
  Eigen::Vector3i color;
  Eigen::Vector2d pt;
  ptrdiff_t cid, fid;
  for (ptrdiff_t pid = 0; pid < number_of_pid; pid++) {
    pid_to_cid_fid->at(pid).clear();

    ptrdiff_t number_of_measures;
    f >> xyz[0] >> xyz[1] >> xyz[2] >>
      color[0] >> color[1] >> color[2] >> number_of_measures;
    pid_to_xyz->at(pid) = xyz;
    for (ptrdiff_t m = 0; m < number_of_measures; m++) {
      f >> cid >> fid >> pt[0] >> pt[1];

      pid_to_cid_fid->at(pid)[cid] = fid;

      if (cid_to_keypoint_map->at(cid).cols() <= fid) {
        cid_to_keypoint_map->at(cid).conservativeResize(Eigen::NoChange_t(), fid + 1);
      }
      cid_to_keypoint_map->at(cid).col(fid) = pt;
    }

    if (!f.good())
      vw::vw_throw(vw::ArgumentErr() << "Unable to correctly read PID: " << pid);
  }
}

// Write an nvm file. Note that a single focal length is assumed and no distortion.
// Those are ignored, and only camera poses, matches, and keypoints are used.
void WriteNVM(std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map,
              std::vector<std::string> const& cid_to_filename,
              std::vector<double> const& focal_lengths,
              std::vector<std::map<int, int>> const& pid_to_cid_fid,
              std::vector<Eigen::Vector3d> const& pid_to_xyz,
              std::vector<Eigen::Affine3d> const& cid_to_cam_t_global,
              std::string const& output_filename) {

  // Ensure that the output directory having this file exists
  vw::create_out_dir(output_filename);

  vw::vw_out() << "Writing: " << output_filename << std::endl;

  std::fstream f(output_filename, std::ios::out);
  f.precision(17); // double precision
  f << "NVM_V3\n";

  if (cid_to_filename.size() != cid_to_keypoint_map.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of filenames and keypoints.");
  if (pid_to_cid_fid.size() != pid_to_xyz.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of pid_to_cid_fid and xyz measurements.");
  if (cid_to_filename.size() != cid_to_cam_t_global.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of filename and camera transforms.");

  // Write camera information
  f << cid_to_filename.size() << std::endl;
  for (size_t cid = 0; cid < cid_to_filename.size(); cid++) {

    // World-to-camera rotation quaternion
    Eigen::Quaterniond q(cid_to_cam_t_global[cid].rotation());

    // Camera center in world coordinates
    Eigen::Vector3d t(cid_to_cam_t_global[cid].translation());
    Eigen::Vector3d camera_center =
      - cid_to_cam_t_global[cid].rotation().inverse() * t;

    f << cid_to_filename[cid] << " " << focal_lengths[cid]
      << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
      << camera_center[0] << " " << camera_center[1] << " "
      << camera_center[2] << " " << "0 0\n"; // zero distortion, not used
  }

  // Write the number of points
  f << pid_to_cid_fid.size() << std::endl;

  for (size_t pid = 0; pid < pid_to_cid_fid.size(); pid++) {
    f << pid_to_xyz[pid][0] << " " << pid_to_xyz[pid][1] << " "
      << pid_to_xyz[pid][2] << " 0 0 0 "
      << pid_to_cid_fid[pid].size();

    if (pid_to_cid_fid[pid].size() <= 1)
      vw::vw_throw(vw::ArgumentErr() << "PID " << pid << " has "
                   << pid_to_cid_fid[pid].size() << " measurements.");

    for (std::map<int, int>::const_iterator it = pid_to_cid_fid[pid].begin();
         it != pid_to_cid_fid[pid].end(); it++) {
      f << " " << it->first << " " << it->second << " "
        << cid_to_keypoint_map[it->first].col(it->second)[0] << " "
        << cid_to_keypoint_map[it->first].col(it->second)[1];
    }
    f << std::endl;
  }

  // Close the file
  f.flush();
  f.close();
}

} // end namespace asp
<commit_msg>Minor tuning of loading pairwise matches<commit_after>// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <asp/Core/Nvm.h>
#include <vw/Core/Exception.h>
#include <vw/Core/Log.h>
#include <vw/FileIO/FileUtils.h>

#include <fstream>
#include <iostream>

namespace asp {

// A wrapper to carry fewer things around
void ReadNVM(std::string const& input_filename, nvmData & nvm) {
  ReadNVM(input_filename,
          &nvm.cid_to_keypoint_map,
          &nvm.cid_to_filename,
          &nvm.pid_to_cid_fid,
          &nvm.pid_to_xyz,
          &nvm.cid_to_cam_t_global);
}

// Reads the NVM control network format. The interest points may or may not
// be shifted relative to optical center. The user is responsible for knowing that.
void ReadNVM(std::string const& input_filename,
             std::vector<Eigen::Matrix2Xd> * cid_to_keypoint_map,
             std::vector<std::string> * cid_to_filename,
             std::vector<std::map<int, int>> * pid_to_cid_fid,
             std::vector<Eigen::Vector3d> * pid_to_xyz,
             std::vector<Eigen::Affine3d> * cid_to_cam_t_global) {

  std::ifstream f(input_filename, std::ios::in);
  std::string token;
  std::getline(f, token);

  // Assert that we start with our NVM token
  if (token.compare(0, 6, "NVM_V3") != 0) {
    vw::vw_throw(vw::ArgumentErr() << "File doesn't start with NVM token.");
  }

  // Read the number of cameras
  ptrdiff_t number_of_cid;
  f >> number_of_cid;
  if (number_of_cid < 1) {
    vw::vw_throw(vw::ArgumentErr() << "NVM file is missing cameras.");
  }

  // Resize all our structures to support the number of cameras we now expect
  cid_to_keypoint_map->resize(number_of_cid);
  cid_to_filename->resize(number_of_cid);
  cid_to_cam_t_global->resize(number_of_cid);
  for (ptrdiff_t cid = 0; cid < number_of_cid; cid++) {
    // Clear keypoints from map. We'll read these in shortly
    cid_to_keypoint_map->at(cid).resize(Eigen::NoChange_t(), 2);

    // Read the line that contains camera information
    double focal, dist1, dist2;
    Eigen::Quaterniond q;
    Eigen::Vector3d c;
    f >> token >> focal;
    f >> q.w() >> q.x() >> q.y() >> q.z();
    f >> c[0] >> c[1] >> c[2] >> dist1 >> dist2;
    cid_to_filename->at(cid) = token;

    // Solve for t, which is part of the affine transform
    Eigen::Matrix3d r = q.matrix();
    cid_to_cam_t_global->at(cid).linear() = r;
    cid_to_cam_t_global->at(cid).translation() = -r * c;
  }

  // Read the number of points
  ptrdiff_t number_of_pid;
  f >> number_of_pid;
  if (number_of_pid < 1)
    vw::vw_throw(vw::ArgumentErr() << "The NVM file has no triangulated points.");

  // Read the point
  pid_to_cid_fid->resize(number_of_pid);
  pid_to_xyz->resize(number_of_pid);
  Eigen::Vector3d xyz;
  Eigen::Vector3i color;
  Eigen::Vector2d pt;
  ptrdiff_t cid, fid;
  for (ptrdiff_t pid = 0; pid < number_of_pid; pid++) {
    pid_to_cid_fid->at(pid).clear();

    ptrdiff_t number_of_measures;
    f >> xyz[0] >> xyz[1] >> xyz[2] >>
      color[0] >> color[1] >> color[2] >> number_of_measures;
    pid_to_xyz->at(pid) = xyz;
    for (ptrdiff_t m = 0; m < number_of_measures; m++) {
      f >> cid >> fid >> pt[0] >> pt[1];

      pid_to_cid_fid->at(pid)[cid] = fid;

      if (cid_to_keypoint_map->at(cid).cols() <= fid) {
        cid_to_keypoint_map->at(cid).conservativeResize(Eigen::NoChange_t(), fid + 1);
      }
      cid_to_keypoint_map->at(cid).col(fid) = pt;
    }

    if (!f.good())
      vw::vw_throw(vw::ArgumentErr() << "Unable to correctly read PID: " << pid);
  }
}

// Write an nvm file. Note that a single focal length is assumed and no distortion.
// Those are ignored, and only camera poses, matches, and keypoints are used.
void WriteNVM(std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map,
              std::vector<std::string> const& cid_to_filename,
              std::vector<double> const& focal_lengths,
              std::vector<std::map<int, int>> const& pid_to_cid_fid,
              std::vector<Eigen::Vector3d> const& pid_to_xyz,
              std::vector<Eigen::Affine3d> const& cid_to_cam_t_global,
              std::string const& output_filename) {

  // Ensure that the output directory having this file exists
  vw::create_out_dir(output_filename);

  vw::vw_out() << "Writing: " << output_filename << std::endl;

  std::fstream f(output_filename, std::ios::out);
  f.precision(17); // double precision
  f << "NVM_V3\n";

  if (cid_to_filename.size() != cid_to_keypoint_map.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of filenames and keypoints.");
  if (pid_to_cid_fid.size() != pid_to_xyz.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of pid_to_cid_fid and xyz measurements.");
  if (cid_to_filename.size() != cid_to_cam_t_global.size())
    vw::vw_throw(vw::ArgumentErr() << "Unequal number of filename and camera transforms.");

  // Write camera information
  f << cid_to_filename.size() << std::endl;
  for (size_t cid = 0; cid < cid_to_filename.size(); cid++) {

    // World-to-camera rotation quaternion
    Eigen::Quaterniond q(cid_to_cam_t_global[cid].rotation());

    // Camera center in world coordinates
    Eigen::Vector3d t(cid_to_cam_t_global[cid].translation());
    Eigen::Vector3d camera_center =
      - cid_to_cam_t_global[cid].rotation().inverse() * t;

    f << cid_to_filename[cid] << " " << focal_lengths[cid]
      << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
      << camera_center[0] << " " << camera_center[1] << " "
      << camera_center[2] << " " << "0 0\n"; // zero distortion, not used
  }

  // Write the number of points
  f << pid_to_cid_fid.size() << std::endl;

  for (size_t pid = 0; pid < pid_to_cid_fid.size(); pid++) {
    f << pid_to_xyz[pid][0] << " " << pid_to_xyz[pid][1] << " "
      << pid_to_xyz[pid][2] << " 0 0 0 "
      << pid_to_cid_fid[pid].size();

    if (pid_to_cid_fid[pid].size() <= 1)
      vw::vw_throw(vw::ArgumentErr() << "PID " << pid << " has "
                   << pid_to_cid_fid[pid].size() << " measurements.");

    for (std::map<int, int>::const_iterator it = pid_to_cid_fid[pid].begin();
         it != pid_to_cid_fid[pid].end(); it++) {
      f << " " << it->first << " " << it->second << " "
        << cid_to_keypoint_map[it->first].col(it->second)[0] << " "
        << cid_to_keypoint_map[it->first].col(it->second)[1];
    }
    f << std::endl;
  }

  // Close the file
  f.flush();
  f.close();
}

} // end namespace asp
<|endoftext|>


<commit_before>#ifndef __Data_student__
#define __Data_student__

#include "data-general.hpp"
#include "data-major.hpp"
#include "data-course.hpp"
using namespace std;

class Semester {
private:
	int period;
public:
	Semester() {
		period = 0;
	}
	Semester(int p) {
		period = p;
	}

	vector<Course> courses;
};

class Student {
private:
	void init(string n, int s, int g, string m) {
		name = n;
		startingYear = s;
		gradutationYear = g;
		// parseMajors(m);

		year1.push_back(Semester(1));
		year1.push_back(Semester(2));
		year1.push_back(Semester(3));

		year2.push_back(Semester(1));
		year2.push_back(Semester(2));
		year2.push_back(Semester(3));

		year3.push_back(Semester(1));
		year3.push_back(Semester(2));
		year3.push_back(Semester(3));

		year4.push_back(Semester(1));
		year4.push_back(Semester(2));
		year4.push_back(Semester(3));
	}
public:
	string name;
	int startingYear, gradutationYear;

	vector<Major> majors;

	vector<Semester> year1;//(3);
	vector<Semester> year2;//(3);
	vector<Semester> year3;//(3);
	vector<Semester> year4;//(3);

	Student() {
		init("", 2000, 2004, "");
	}
	Student(string fn) {
		ifstream infile;
		infile.open(fn.c_str());
	}

	bool hasTakenCourse() {
		return false;
	}

	ostream& getData(ostream &os) {
		os << name << " ";
		return os;
	}
	void display();
};

#endif
<commit_msg>Initial attempt at addCourse()<commit_after>#ifndef __Data_student__
#define __Data_student__

#include "data-general.hpp"
#include "data-major.hpp"
#include "data-course.hpp"
using namespace std;

class Semester {
private:
	int period;
public:
	Semester() {
		period = 0;
	}
	Semester(int p) {
		period = p;
	}

	vector<Course> courses;
};

class Student {
private:
	void init(string n, int s, int g, string m) {
		name = n;
		startingYear = s;
		gradutationYear = g;
		// parseMajors(m);

		year1.push_back(Semester(1));
		year1.push_back(Semester(2));
		year1.push_back(Semester(3));

		year2.push_back(Semester(1));
		year2.push_back(Semester(2));
		year2.push_back(Semester(3));

		year3.push_back(Semester(1));
		year3.push_back(Semester(2));
		year3.push_back(Semester(3));

		year4.push_back(Semester(1));
		year4.push_back(Semester(2));
		year4.push_back(Semester(3));
	}
public:
	string name;
	int startingYear, gradutationYear;

	vector<Major> majors;

	vector<Semester> year1;//(3);
	vector<Semester> year2;//(3);
	vector<Semester> year3;//(3);
	vector<Semester> year4;//(3);

	Student() {
		init("", 2000, 2004, "");
	}
	Student(string fn) {
		ifstream infile;
		infile.open(fn.c_str());
	}

	bool hasTakenCourse() {
		return false;
	}

	void addCourse(const Course& c, const Semester& s) {
		// s.push_back();
	}

	ostream& getData(ostream &os) {
		os << name << " ";
		return os;
	}
	void display();
};

#endif
<|endoftext|>