<commit_before>/* exception_handler.cc
   Jeremy Barnes, 26 February 2008
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

*/

#include <cxxabi.h>
#include <cstring>
#include <fstream>

#include "jml/compiler/compiler.h"
#include "jml/utils/environment.h"

#include "backtrace.h"
#include "demangle.h"
#include "exception.h"
#include "exception_hook.h"
#include "format.h"
#include "threads.h"

using namespace std;

namespace ML {

Env_Option<bool> TRACE_EXCEPTIONS("JML_TRACE_EXCEPTIONS", true);

__thread bool trace_exceptions = false;
__thread bool trace_exceptions_initialized = false;

void set_default_trace_exceptions(bool val)
{
    TRACE_EXCEPTIONS.set(val);
}

bool get_default_trace_exceptions()
{
    return TRACE_EXCEPTIONS;
}

void set_trace_exceptions(bool trace)
{
    //cerr << "set_trace_exceptions to " << trace << " at " << &trace_exceptions
    //     << endl;
    trace_exceptions = trace;
    trace_exceptions_initialized = true;
}

bool get_trace_exceptions()
{
    if (!trace_exceptions_initialized) {
        //cerr << "trace_exceptions initialized to = "
        //     << trace_exceptions << " at " << &trace_exceptions << endl;
        set_trace_exceptions(TRACE_EXCEPTIONS);
        trace_exceptions_initialized = true;
    }

    //cerr << "get_trace_exceptions returned " << trace_exceptions
    //     << " at " << &trace_exceptions << endl;

    return trace_exceptions;
}

static const std::exception *
to_std_exception(void* object, const std::type_info * tinfo)
{
    /* Check if its a class.  If not, we can't see if it's a std::exception.
       The abi::__class_type_info is the base class of all types of type
       info for types that are classes (of which std::exception is one).
    */
    const abi::__class_type_info * ctinfo
        = dynamic_cast<const abi::__class_type_info *>(tinfo);

    if (!ctinfo) return 0;

    /* The thing thrown was an object.  Now, check if it is derived from
    std::exception. */
    const std::type_info * etinfo = &typeid(std::exception);

    /* See if the exception could catch this.  This is the mechanism
    used internally by the compiler in catch {} blocks to see if
    the exception matches the catch type.

    In the case of success, the object will be adjusted to point to
    the start of the std::exception object.
    */
    void * obj_ptr = object;
    bool can_catch = etinfo->__do_catch(tinfo, &obj_ptr, 0);

    if (!can_catch) return 0;

    /* obj_ptr points to a std::exception; extract it and get the
    exception message.
    */
    return (const std::exception *)obj_ptr;
}

/** We install this handler for when an exception is thrown. */

void default_exception_tracer(void * object, const std::type_info * tinfo)
{
    //cerr << "trace_exception: trace_exceptions = " << get_trace_exceptions()
    //     << " at " << &trace_exceptions << endl;

    if (!get_trace_exceptions()) return;

    const std::exception * exc = to_std_exception(object, tinfo);

    // We don't want these exceptions to be printed out.
    if (dynamic_cast<const ML::SilentException *>(exc)) return;

    /* avoid allocations when std::bad_alloc is thrown */
    bool noAlloc = dynamic_cast<const std::bad_alloc *>(exc);

    size_t bufferSize(1024*1024);
    char buffer[bufferSize];
    char datetime[128];
    size_t totalWritten(0), written, remaining(bufferSize);

    time_t now;
    time(&now);

    struct tm lt_tm;
    strftime(datetime, sizeof(datetime), "%FT%H:%M:%SZ",
             gmtime_r(&now, &lt_tm));

    const char * demangled;
    char * heapDemangled;
    if (noAlloc) {
        heapDemangled = nullptr;
        demangled = "std::bad_alloc";
    }
    else {
        heapDemangled = char_demangle(tinfo->name());
        demangled = heapDemangled;
    }
    auto pid = getpid();
    auto tid = gettid();

    written = ::snprintf(buffer, remaining,
                         "\n"
                         "--------------------------[Exception thrown]"
                         "---------------------------\n"
                         "time:   %s\n"
                         "type:   %s\n"
                         "pid:    %d; tid: %d\n",
                         datetime, demangled, pid, tid);
    if (heapDemangled) {
        free(heapDemangled);
    }
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;
    remaining -= written;

    if (exc) {
        written = snprintf(buffer + totalWritten, remaining,
                           "what:   %s\n", exc->what());
        if (written >= remaining) {
            goto end;
        }
        totalWritten += written;
        remaining -= written;
    }

    if (noAlloc) {
        goto end;
    }

    written = snprintf(buffer + totalWritten, remaining, "stack:\n");
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;
    remaining -= written;

    written = backtrace(buffer + totalWritten, remaining, 3);
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;

    if (totalWritten < bufferSize - 1) {
        strcpy(buffer + totalWritten, "\n");
    }

end:
    cerr << buffer;

    char const * reports = getenv("ENABLE_EXCEPTION_REPORTS");
    if (!noAlloc && reports) {
        std::string path = ML::format("%s/exception-report-%s-%d-%d.log",
                                      reports, datetime, pid, tid);

        std::ofstream file(path, std::ios_base::app);
        if(file) {
            file << getenv("_") << endl;
            backtrace(file, 3);
            file.close();
        }
    }
}

} // namespace ML
<commit_msg>PLAT-887: include node name in exception message<commit_after>/* exception_handler.cc
   Jeremy Barnes, 26 February 2008
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

*/

#include <sys/utsname.h>
#include <cxxabi.h>
#include <cstring>
#include <fstream>

#include "jml/compiler/compiler.h"
#include "jml/utils/environment.h"

#include "backtrace.h"
#include "demangle.h"
#include "exception.h"
#include "exception_hook.h"
#include "format.h"
#include "threads.h"

using namespace std;

namespace ML {

Env_Option<bool> TRACE_EXCEPTIONS("JML_TRACE_EXCEPTIONS", true);

__thread bool trace_exceptions = false;
__thread bool trace_exceptions_initialized = false;

void set_default_trace_exceptions(bool val)
{
    TRACE_EXCEPTIONS.set(val);
}

bool get_default_trace_exceptions()
{
    return TRACE_EXCEPTIONS;
}

void set_trace_exceptions(bool trace)
{
    //cerr << "set_trace_exceptions to " << trace << " at " << &trace_exceptions
    //     << endl;
    trace_exceptions = trace;
    trace_exceptions_initialized = true;
}

bool get_trace_exceptions()
{
    if (!trace_exceptions_initialized) {
        //cerr << "trace_exceptions initialized to = "
        //     << trace_exceptions << " at " << &trace_exceptions << endl;
        set_trace_exceptions(TRACE_EXCEPTIONS);
        trace_exceptions_initialized = true;
    }

    //cerr << "get_trace_exceptions returned " << trace_exceptions
    //     << " at " << &trace_exceptions << endl;

    return trace_exceptions;
}

static const std::exception *
to_std_exception(void* object, const std::type_info * tinfo)
{
    /* Check if its a class.  If not, we can't see if it's a std::exception.
       The abi::__class_type_info is the base class of all types of type
       info for types that are classes (of which std::exception is one).
    */
    const abi::__class_type_info * ctinfo
        = dynamic_cast<const abi::__class_type_info *>(tinfo);

    if (!ctinfo) return 0;

    /* The thing thrown was an object.  Now, check if it is derived from
    std::exception. */
    const std::type_info * etinfo = &typeid(std::exception);

    /* See if the exception could catch this.  This is the mechanism
    used internally by the compiler in catch {} blocks to see if
    the exception matches the catch type.

    In the case of success, the object will be adjusted to point to
    the start of the std::exception object.
    */
    void * obj_ptr = object;
    bool can_catch = etinfo->__do_catch(tinfo, &obj_ptr, 0);

    if (!can_catch) return 0;

    /* obj_ptr points to a std::exception; extract it and get the
    exception message.
    */
    return (const std::exception *)obj_ptr;
}

/** We install this handler for when an exception is thrown. */

void default_exception_tracer(void * object, const std::type_info * tinfo)
{
    //cerr << "trace_exception: trace_exceptions = " << get_trace_exceptions()
    //     << " at " << &trace_exceptions << endl;

    if (!get_trace_exceptions()) return;

    const std::exception * exc = to_std_exception(object, tinfo);

    // We don't want these exceptions to be printed out.
    if (dynamic_cast<const ML::SilentException *>(exc)) return;

    /* avoid allocations when std::bad_alloc is thrown */
    bool noAlloc = dynamic_cast<const std::bad_alloc *>(exc);

    size_t bufferSize(1024*1024);
    char buffer[bufferSize];
    char datetime[128];
    size_t totalWritten(0), written, remaining(bufferSize);

    time_t now;
    time(&now);

    struct tm lt_tm;
    strftime(datetime, sizeof(datetime), "%FT%H:%M:%SZ",
             gmtime_r(&now, &lt_tm));

    const char * demangled;
    char * heapDemangled;
    if (noAlloc) {
        heapDemangled = nullptr;
        demangled = "std::bad_alloc";
    }
    else {
        heapDemangled = char_demangle(tinfo->name());
        demangled = heapDemangled;
    }
    const char *nodeName = "<unknown>";
    struct utsname utsName;
    if (::uname(&utsName) == 0) {
        nodeName = utsName.nodename;
    }
    else {
        cerr << "error calling uname\n";
    }
    auto pid = getpid();
    auto tid = gettid();

    written = ::snprintf(buffer, remaining,
                         "\n"
                         "--------------------------[Exception thrown]"
                         "---------------------------\n"
                         "time:   %s\n"
                         "type:   %s\n"
                         "node:   %s\n"
                         "pid:    %d; tid: %d\n",
                         datetime, demangled, nodeName, pid, tid);
    if (heapDemangled) {
        free(heapDemangled);
    }
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;
    remaining -= written;

    if (exc) {
        written = snprintf(buffer + totalWritten, remaining,
                           "what:   %s\n", exc->what());
        if (written >= remaining) {
            goto end;
        }
        totalWritten += written;
        remaining -= written;
    }

    if (noAlloc) {
        goto end;
    }

    written = snprintf(buffer + totalWritten, remaining, "stack:\n");
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;
    remaining -= written;

    written = backtrace(buffer + totalWritten, remaining, 3);
    if (written >= remaining) {
        goto end;
    }
    totalWritten += written;

    if (totalWritten < bufferSize - 1) {
        strcpy(buffer + totalWritten, "\n");
    }

end:
    cerr << buffer;

    char const * reports = getenv("ENABLE_EXCEPTION_REPORTS");
    if (!noAlloc && reports) {
        std::string path = ML::format("%s/exception-report-%s-%d-%d.log",
                                      reports, datetime, pid, tid);

        std::ofstream file(path, std::ios_base::app);
        if(file) {
            file << getenv("_") << endl;
            backtrace(file, 3);
            file.close();
        }
    }
}

} // namespace ML
<|endoftext|>


<commit_before>// PythonStuff.cpp

// written by Dan Heeks, February 6th 2009, license: GPL version 3 http://www.gnu.org/licenses/gpl-3.0.txt

#include "PythonStuff.h"

#include <set>

#include "Area.h"

#if _DEBUG
#undef _DEBUG
#include <Python.h>
#define _DEBUG
#else
#include <Python.h>
#endif

#ifdef __GNUG__
#pragma implementation
#endif

#include "kbool/include/_lnk_itr.h"

#include "kbool/include/booleng.h"

std::set<CArea*> valid_areas;

static PyObject* area_new(PyObject* self, PyObject* args)
{
	CArea* new_object = new CArea();
	valid_areas.insert(new_object);

	// return new object cast to an int
	PyObject *pValue = PyInt_FromLong((long)new_object);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_exists(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	bool exists = (valid_areas.find(k) != valid_areas.end());

	// return exists
	PyObject *pValue = exists ? Py_True : Py_False;
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_delete(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		delete k;
		valid_areas.erase(k);
	}

	Py_RETURN_NONE;
}

static int span_number = 1;

static PyObject* area_add_point(PyObject* self, PyObject* args)
{
	double x, y, i, j;
	int sp, ik;
	if (!PyArg_ParseTuple(args, "iidddd", &ik, &sp, &x, &y, &i, &j)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		// add a curve if there isn't one
		if(k->m_curves.size() == 0)k->m_curves.push_back(CCurve());
		int curve_index = k->m_curves.size() - 1;

		// can't add arc as first span
		if(sp && k->m_curves[curve_index].m_vertices.size() == 0){ const char* str = "can't add arc to area as first point"; printf(str); throw(str);}

		// add the vertex
		k->m_curves[curve_index].m_vertices.push_back(CVertex(sp, x, y, i, j, span_number));
		span_number++;
	}

	Py_RETURN_NONE;
}

static PyObject* area_start_new_curve(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		// add a new curve
		k->m_curves.push_back(CCurve());
	}

	Py_RETURN_NONE;
}

static PyObject* area_offset(PyObject* self, PyObject* args)
{
	int ik;
	double inwards;
	if (!PyArg_ParseTuple(args, "id", &ik, &inwards)) return NULL;

	CArea* k = (CArea*)ik;
	//int ret = 0;

	if(valid_areas.find(k) != valid_areas.end())
	{
		k->Offset(inwards);
	}

	Py_RETURN_NONE;
}

static PyObject* area_subtract(PyObject* self, PyObject* args)
{
	int a1, a2;
	if (!PyArg_ParseTuple(args, "ii", &a1, &a2)) return NULL;

	CArea* area1 = (CArea*)a1;
	CArea* area2 = (CArea*)a2;

	if(valid_areas.find(area1) != valid_areas.end() && valid_areas.find(area2) != valid_areas.end())
	{
		area1->Subtract(*area2);
	}

	Py_RETURN_NONE;
}

static PyObject* area_set_round_corner_factor(PyObject* self, PyObject* args)
{
	if (!PyArg_ParseTuple(args, "d", &CArea::m_round_corners_factor)) return NULL;

	Py_RETURN_NONE;
}

static PyObject* area_copy(PyObject* self, PyObject* args)
{
	int ik;
	int ik2;
	if (!PyArg_ParseTuple(args, "ii", &ik, &ik2)) return NULL;
	CArea* k = (CArea*)ik;
	CArea* k2 = (CArea*)ik2;
	if(valid_areas.find(k) != valid_areas.end())
	{
		if(valid_areas.find(k2) != valid_areas.end())
		{
			*k2 = *k;
		}
	}

	Py_RETURN_NONE;
}

static PyObject* area_num_curves(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;
	CArea* k = (CArea*)ik;

	int n = 0;
	if(valid_areas.find(k) != valid_areas.end())
	{
		n = k->m_curves.size();
	}

	PyObject *pValue = PyInt_FromLong(n);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_num_vertices(PyObject* self, PyObject* args)
{
	int ik, ic;
	if (!PyArg_ParseTuple(args, "ii", &ik, &ic)) return NULL;
	CArea* k = (CArea*)ik;

	int n = 0;
	if(valid_areas.find(k) != valid_areas.end())
	{
		if(ic >= 0 && ic < (int)(k->m_curves.size()))n = k->m_curves[ic].m_vertices.size();
	}

	PyObject *pValue = PyInt_FromLong(n);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_get_vertex(PyObject* self, PyObject* args)
{
	int ik, ic;
	int index; // 0 is first
	if (!PyArg_ParseTuple(args, "iii", &ik, &ic, &index)) return NULL;
	CArea* k = (CArea*)ik;

	int sp = 0;
	double x = 0.0, y = 0.0;
	double cx = 0.0, cy = 0.0;

	if(valid_areas.find(k) != valid_areas.end())
	{
		if(ic >= 0 && ic < (int)(k->m_curves.size()))
		{
			CCurve& curve = k->m_curves[ic];
			if(index >= 0 && index < (int)(curve.m_vertices.size()))
			{
				CVertex& vertex = curve.m_vertices[index];
				sp = vertex.m_type;
				x = vertex.m_p[0];
				y = vertex.m_p[1];
				cx = vertex.m_c[0];
				cy = vertex.m_c[1];
			}
		}
	}

	// return span data as a tuple
	PyObject *pTuple = PyTuple_New(5);
	{
		PyObject *pValue = PyInt_FromLong(sp);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 0, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(x);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 1, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(y);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 2, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(cx);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 3, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(cy);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 4, pValue);
	}

	Py_INCREF(pTuple);
	return pTuple;
}

static PyObject* area_add_curve(PyObject* self, PyObject* args)
{
	int ik, ij, ii;
	if (!PyArg_ParseTuple(args, "iii", &ik, &ij, &ii)) return NULL;

	CArea* k = (CArea*)ik;
	CArea* j = (CArea*)ij;
	if(valid_areas.find(k) != valid_areas.end() && valid_areas.find(j) != valid_areas.end())
	{
        if(ii >= 0 && ii < (int)j->m_curves.size()){
            // add curve
            k->m_curves.push_back(j->m_curves[ii]);
        }
	}

	Py_RETURN_NONE;
}

static PyMethodDef AreaMethods[] = {
	{"new", area_new, METH_VARARGS , ""},
	{"exists", area_exists, METH_VARARGS , ""},
	{"delete", area_delete, METH_VARARGS , ""},
	{"add_point", area_add_point, METH_VARARGS , ""},
	{"start_new_curve", area_start_new_curve, METH_VARARGS , ""},
	{"subtract", area_subtract, METH_VARARGS , ""},
	{"offset", area_offset, METH_VARARGS , ""},
	{"set_round_corner_factor", area_set_round_corner_factor, METH_VARARGS , ""},
	{"copy", area_copy, METH_VARARGS , ""},
	{"num_curves", area_num_curves, METH_VARARGS , ""},
	{"num_vertices", area_num_vertices, METH_VARARGS , ""},
	{"get_vertex", area_get_vertex, METH_VARARGS , ""},
	{"add_curve", area_add_curve, METH_VARARGS , ""},
	{NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC
initarea(void)
{
	Py_InitModule("area", AreaMethods);
}
<commit_msg>added area.print_area(a)<commit_after>// PythonStuff.cpp

// written by Dan Heeks, February 6th 2009, license: GPL version 3 http://www.gnu.org/licenses/gpl-3.0.txt

#include "PythonStuff.h"

#include <set>

#include "Area.h"

#if _DEBUG
#undef _DEBUG
#include <Python.h>
#define _DEBUG
#else
#include <Python.h>
#endif

#ifdef __GNUG__
#pragma implementation
#endif

#include "kbool/include/_lnk_itr.h"

#include "kbool/include/booleng.h"

std::set<CArea*> valid_areas;

static PyObject* area_new(PyObject* self, PyObject* args)
{
	CArea* new_object = new CArea();
	valid_areas.insert(new_object);

	// return new object cast to an int
	PyObject *pValue = PyInt_FromLong((long)new_object);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_exists(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	bool exists = (valid_areas.find(k) != valid_areas.end());

	// return exists
	PyObject *pValue = exists ? Py_True : Py_False;
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_delete(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		delete k;
		valid_areas.erase(k);
	}

	Py_RETURN_NONE;
}

static int span_number = 1;

static PyObject* area_add_point(PyObject* self, PyObject* args)
{
	double x, y, i, j;
	int sp, ik;
	if (!PyArg_ParseTuple(args, "iidddd", &ik, &sp, &x, &y, &i, &j)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		// add a curve if there isn't one
		if(k->m_curves.size() == 0)k->m_curves.push_back(CCurve());
		int curve_index = k->m_curves.size() - 1;

		// can't add arc as first span
		if(sp && k->m_curves[curve_index].m_vertices.size() == 0){ const char* str = "can't add arc to area as first point"; printf(str); throw(str);}

		// add the vertex
		k->m_curves[curve_index].m_vertices.push_back(CVertex(sp, x, y, i, j, span_number));
		span_number++;
	}

	Py_RETURN_NONE;
}

static PyObject* area_start_new_curve(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;

	CArea* k = (CArea*)ik;
	if(valid_areas.find(k) != valid_areas.end())
	{
		// add a new curve
		k->m_curves.push_back(CCurve());
	}

	Py_RETURN_NONE;
}

static PyObject* area_offset(PyObject* self, PyObject* args)
{
	int ik;
	double inwards;
	if (!PyArg_ParseTuple(args, "id", &ik, &inwards)) return NULL;

	CArea* k = (CArea*)ik;
	//int ret = 0;

	if(valid_areas.find(k) != valid_areas.end())
	{
		k->Offset(inwards);
	}

	Py_RETURN_NONE;
}

static PyObject* area_subtract(PyObject* self, PyObject* args)
{
	int a1, a2;
	if (!PyArg_ParseTuple(args, "ii", &a1, &a2)) return NULL;

	CArea* area1 = (CArea*)a1;
	CArea* area2 = (CArea*)a2;

	if(valid_areas.find(area1) != valid_areas.end() && valid_areas.find(area2) != valid_areas.end())
	{
		area1->Subtract(*area2);
	}

	Py_RETURN_NONE;
}

static PyObject* area_set_round_corner_factor(PyObject* self, PyObject* args)
{
	if (!PyArg_ParseTuple(args, "d", &CArea::m_round_corners_factor)) return NULL;

	Py_RETURN_NONE;
}

static PyObject* area_copy(PyObject* self, PyObject* args)
{
	int ik;
	int ik2;
	if (!PyArg_ParseTuple(args, "ii", &ik, &ik2)) return NULL;
	CArea* k = (CArea*)ik;
	CArea* k2 = (CArea*)ik2;
	if(valid_areas.find(k) != valid_areas.end())
	{
		if(valid_areas.find(k2) != valid_areas.end())
		{
			*k2 = *k;
		}
	}

	Py_RETURN_NONE;
}

static PyObject* area_num_curves(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;
	CArea* k = (CArea*)ik;

	int n = 0;
	if(valid_areas.find(k) != valid_areas.end())
	{
		n = k->m_curves.size();
	}

	PyObject *pValue = PyInt_FromLong(n);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_num_vertices(PyObject* self, PyObject* args)
{
	int ik, ic;
	if (!PyArg_ParseTuple(args, "ii", &ik, &ic)) return NULL;
	CArea* k = (CArea*)ik;

	int n = 0;
	if(valid_areas.find(k) != valid_areas.end())
	{
		if(ic >= 0 && ic < (int)(k->m_curves.size()))n = k->m_curves[ic].m_vertices.size();
	}

	PyObject *pValue = PyInt_FromLong(n);
	Py_INCREF(pValue);
	return pValue;
}

static PyObject* area_get_vertex(PyObject* self, PyObject* args)
{
	int ik, ic;
	int index; // 0 is first
	if (!PyArg_ParseTuple(args, "iii", &ik, &ic, &index)) return NULL;
	CArea* k = (CArea*)ik;

	int sp = 0;
	double x = 0.0, y = 0.0;
	double cx = 0.0, cy = 0.0;

	if(valid_areas.find(k) != valid_areas.end())
	{
		if(ic >= 0 && ic < (int)(k->m_curves.size()))
		{
			CCurve& curve = k->m_curves[ic];
			if(index >= 0 && index < (int)(curve.m_vertices.size()))
			{
				CVertex& vertex = curve.m_vertices[index];
				sp = vertex.m_type;
				x = vertex.m_p[0];
				y = vertex.m_p[1];
				cx = vertex.m_c[0];
				cy = vertex.m_c[1];
			}
		}
	}

	// return span data as a tuple
	PyObject *pTuple = PyTuple_New(5);
	{
		PyObject *pValue = PyInt_FromLong(sp);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 0, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(x);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 1, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(y);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 2, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(cx);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 3, pValue);
	}
	{
		PyObject *pValue = PyFloat_FromDouble(cy);
		if (!pValue){
			Py_DECREF(pTuple);return NULL;
		}
		PyTuple_SetItem(pTuple, 4, pValue);
	}

	Py_INCREF(pTuple);
	return pTuple;
}

static PyObject* area_add_curve(PyObject* self, PyObject* args)
{
	int ik, ij, ii;
	if (!PyArg_ParseTuple(args, "iii", &ik, &ij, &ii)) return NULL;

	CArea* k = (CArea*)ik;
	CArea* j = (CArea*)ij;
	if(valid_areas.find(k) != valid_areas.end() && valid_areas.find(j) != valid_areas.end())
	{
        if(ii >= 0 && ii < (int)j->m_curves.size()){
            // add curve
            k->m_curves.push_back(j->m_curves[ii]);
        }
	}

	Py_RETURN_NONE;
}

static void print_curve(const CCurve& c)
{
	unsigned int nvertices = c.m_vertices.size();
	printf("number of vertices = %d\n", nvertices);
	for(unsigned int i = 0; i< nvertices; i++)
	{
		const CVertex &vertex = c.m_vertices[i];
		printf("vertex %d type = %d, x = %g, y = %g", i+1, vertex.m_type, vertex.m_p[0], vertex.m_p[1]);
		if(vertex.m_type)printf(", xc = %g, yc = %g", vertex.m_c[0], vertex.m_c[1]);
		printf("\n");
	}
}

static void print_area(const CArea &a)
{
	for(unsigned int i = 0; i<a.m_curves.size(); i++)
	{
		const CCurve& curve = a.m_curves[i];
		print_curve(curve);
	}
}

static PyObject* area_print_area(PyObject* self, PyObject* args)
{
	int ik;
	if (!PyArg_ParseTuple(args, "i", &ik)) return NULL;
	CArea* k = (CArea*)ik;

	if(valid_areas.find(k) != valid_areas.end())
	{
		print_area(*k);
	}

	Py_RETURN_NONE;
}

static PyMethodDef AreaMethods[] = {
	{"new", area_new, METH_VARARGS , ""},
	{"exists", area_exists, METH_VARARGS , ""},
	{"delete", area_delete, METH_VARARGS , ""},
	{"add_point", area_add_point, METH_VARARGS , ""},
	{"start_new_curve", area_start_new_curve, METH_VARARGS , ""},
	{"subtract", area_subtract, METH_VARARGS , ""},
	{"offset", area_offset, METH_VARARGS , ""},
	{"set_round_corner_factor", area_set_round_corner_factor, METH_VARARGS , ""},
	{"copy", area_copy, METH_VARARGS , ""},
	{"num_curves", area_num_curves, METH_VARARGS , ""},
	{"num_vertices", area_num_vertices, METH_VARARGS , ""},
	{"get_vertex", area_get_vertex, METH_VARARGS , ""},
	{"add_curve", area_add_curve, METH_VARARGS , ""},
	{"print_area", area_print_area, METH_VARARGS , ""},
	{NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC
initarea(void)
{
	Py_InitModule("area", AreaMethods);
}
<|endoftext|>


<commit_before>#include "ExampleAndroidHandler.h"
#include <AndroidLog.h>

ExampleAndroidHandler::ExampleAndroidHandler()
{
	// State variables
	m_bShouldQuit 	= false;
	m_bIsVisible 	= false;
	m_bIsPaused		= true;

	// Egl
	m_Display = EGL_NO_DISPLAY;
	m_Surface = EGL_NO_SURFACE;
	m_Context = NULL;
}

ExampleAndroidHandler::~ExampleAndroidHandler()
{
	DestroyOpenGL();
}

void ExampleAndroidHandler::Run()
{
	// Example asset read
	Android::Asset* pAsset = Android::GetAssetManager().GetAsset( "test.txt" );
	if ( pAsset )
	{
		// Create a buffer to read the content into,
		// [ Size() + 1 ] for null terminator character for LOV usage
		char* pBuffer = new char[ pAsset->Size() + 1 ];

		// Read the buffer
		pAsset->Read( pBuffer, pAsset->Size() );

		// Delete the asset file
		delete pAsset;

		// Set null terminating for LOGV
		pBuffer[ pAsset->Size() ] = 0;

		// Show us the file's content!
		LOGV( "File content: %s", pBuffer );

		// Delete the buffer
		delete [] pBuffer;
	}

	// Create time measurement
	timespec timeNow;
	clock_gettime( CLOCK_MONOTONIC, &timeNow );
	uint64_t uPreviousTime = timeNow.tv_sec * 1000000000ull + timeNow.tv_nsec;

	// While application is alive...
	while ( !m_bShouldQuit )
	{
		// Handle Android events
		Android::PollEvents();

		// Calculate delta time
		clock_gettime( CLOCK_MONOTONIC, &timeNow ); // query time now
		uint64_t uNowNano = timeNow.tv_sec * 1000000000ull + timeNow.tv_nsec; // get time in nanoseconds
		float fDeltaSeconds = float( uNowNano - uPreviousTime ) * 0.000000001f; // 1 second = 1,000,000,000 nanoseconds
		uPreviousTime = uNowNano; // set previous time to new time

		// If not paused...
		if ( !m_bIsPaused )
		{
			// Update logic
			Update( fDeltaSeconds );
		}

		// If visible
		if ( m_bIsVisible )
		{
			// Draw
			Draw();
		}

	}
}

void ExampleAndroidHandler::Update( float fDeltaSeconds )
{
	// Do stuff here
}

void ExampleAndroidHandler::Draw()
{
	// Draw things here
	glClearColor( 0.0f, 0.0f, 1.0f, 1.0f );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	if ( !eglSwapBuffers( m_Display, m_Surface ) )
	{
		LOGE( "eglSwapBuffers() returned error %d", eglGetError() );
	}
}

bool ExampleAndroidHandler::InitOpenGL()
{
	const EGLint attribs[] =
	{
		EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
		EGL_BLUE_SIZE, 8,
		EGL_GREEN_SIZE, 8,
		EGL_RED_SIZE, 8,
		EGL_NONE
	};

	EGLDisplay display;
	EGLConfig config;
	EGLint numConfigs;
	EGLint format;
	EGLSurface surface;
	EGLContext context;
	EGLint width;
	EGLint height;
	GLfloat ratio;

	LOGV( "[Example]: Initializing context" );

	if ( ( display = eglGetDisplay( EGL_DEFAULT_DISPLAY ) ) == EGL_NO_DISPLAY )
	{
		LOGE( "[Example]: eglGetDisplay() returned error %d", eglGetError() );
		return false;
	}

	if ( !eglInitialize( display, 0, 0 ) )
	{
		LOGE( "[Example]: eglInitialize() returned error %d", eglGetError() );
		return false;
	}

	if ( !eglChooseConfig(display, attribs, &config, 1, &numConfigs) )
	{
		LOGE( "[Example]: eglChooseConfig() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format) )
	{
		LOGE( "[Example]: eglGetConfigAttrib() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	// Set buffer geometry using our window which is saved in Android
	ANativeWindow_setBuffersGeometry( Android::GetWindow(), 0, 0, format );

	if ( !( surface = eglCreateWindowSurface( display, config, Android::GetWindow(), 0 ) ) )
	{
		LOGE( "[Example]: eglCreateWindowSurface() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !( context = eglCreateContext( display, config, 0, 0 ) ) )
	{
		LOGE( "[Example]: eglCreateContext() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglMakeCurrent(display, surface, surface, context ) )
	{
		LOGE( "[Example]: eglMakeCurrent() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglQuerySurface( display, surface, EGL_WIDTH, &width ) ||
		!eglQuerySurface( display, surface, EGL_HEIGHT, &height ) )
	{
		LOGE( "[Example]: eglQuerySurface() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	m_Display = display;
	m_Surface = surface;
	m_Context = context;

	glDisable( GL_DITHER );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST );
	glClearColor(0, 0, 0, 0);
	glEnable( GL_CULL_FACE );
	glEnable( GL_DEPTH_TEST );

	return true;
}

void ExampleAndroidHandler::DestroyOpenGL()
{
	if ( m_Display )
	{
		LOGV( "[Example]: Shutting down OpenGL" );

		eglMakeCurrent( m_Display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT );
		eglDestroyContext( m_Display, m_Context );
		eglDestroySurface( m_Display, m_Surface);
		eglTerminate( m_Display );

		m_Display = EGL_NO_DISPLAY;
		m_Surface = EGL_NO_SURFACE;
		m_Context = NULL;
	}
}

// Application
void ExampleAndroidHandler::OnShutdown()
{
	LOGV( "[Example]: Shutting down!" );
	m_bShouldQuit = true;
}

// Surface
void ExampleAndroidHandler::OnSurfaceCreated()
{
	LOGV( "[Example]: Creating surface!" );
	if ( InitOpenGL() == false )
	{
		m_bShouldQuit = true;
	}
}

void ExampleAndroidHandler::OnSurfaceChanged( int iPixelFormat, int iWidth, int iHeight )
{
	LOGV( "[Example]: Setting viewports!" );

	// Set new viewport
	glViewport( 0, 0, iWidth, iHeight );
}

void ExampleAndroidHandler::OnSurfaceDestroyed()
{
	LOGV( "[Example]: Destroying egl!" );
	DestroyOpenGL();
}

// States
void ExampleAndroidHandler::OnPause()
{
	LOGV( "[Example]: Paused!" );
	m_bIsPaused = true;
}

void ExampleAndroidHandler::OnResume()
{
	LOGV( "[Example]: Resumed!" );
	m_bIsPaused = false;
}

void ExampleAndroidHandler::OnVisible()
{
	LOGV( "[Example]: Visible!" );
	m_bIsVisible = true;
}

void ExampleAndroidHandler::OnHidden()
{
	LOGV( "[Example]: Hidden!" );
	m_bIsVisible = false;
}

void ExampleAndroidHandler::OnLowMemory()
{
	LOGV( "[Example]: Clearing some memory to stay alive..." );

	// BigMemoryObject->Release();
}

// Input
void ExampleAndroidHandler::OnKey( int iKeyCode, wchar_t iUnicodeChar )
{
	LOGV( "[Example]: Got key! %i %c", iKeyCode, iUnicodeChar );
}

void ExampleAndroidHandler::OnTouch( int iPointerID, float fPosX, float fPosY, int iAction )
{
	LOGV( "[Example]: Touch: %i, x: %f y:, %f action:, %i.", iPointerID, fPosX, fPosY, iAction );

	if ( iAction == 0 )
	{
		// On touch start show keyboard!
		Android::ShowKeyboard();
	}

	else if ( iAction == 1 )
	{
		// On touch up, hide keyboard...
		Android::HideKeyboard();
	}
}
<commit_msg>Added missing log identifiers<commit_after>#include "ExampleAndroidHandler.h"
#include <AndroidLog.h>

ExampleAndroidHandler::ExampleAndroidHandler()
{
	// State variables
	m_bShouldQuit 	= false;
	m_bIsVisible 	= false;
	m_bIsPaused		= true;

	// Egl
	m_Display = EGL_NO_DISPLAY;
	m_Surface = EGL_NO_SURFACE;
	m_Context = NULL;
}

ExampleAndroidHandler::~ExampleAndroidHandler()
{
	DestroyOpenGL();
}

void ExampleAndroidHandler::Run()
{
	// Example asset read
	Android::Asset* pAsset = Android::GetAssetManager().GetAsset( "test.txt" );
	if ( pAsset )
	{
		// Create a buffer to read the content into,
		// [ Size() + 1 ] for null terminator character for LOV usage
		char* pBuffer = new char[ pAsset->Size() + 1 ];

		// Read the buffer
		pAsset->Read( pBuffer, pAsset->Size() );

		// Delete the asset file
		delete pAsset;

		// Set null terminating for LOGV
		pBuffer[ pAsset->Size() ] = 0;

		// Show us the file's content!
		LOGV( "[Example]: File content: %s", pBuffer );

		// Delete the buffer
		delete [] pBuffer;
	}

	// Create time measurement
	timespec timeNow;
	clock_gettime( CLOCK_MONOTONIC, &timeNow );
	uint64_t uPreviousTime = timeNow.tv_sec * 1000000000ull + timeNow.tv_nsec;

	// While application is alive...
	while ( !m_bShouldQuit )
	{
		// Handle Android events
		Android::PollEvents();

		// Calculate delta time
		clock_gettime( CLOCK_MONOTONIC, &timeNow ); // query time now
		uint64_t uNowNano = timeNow.tv_sec * 1000000000ull + timeNow.tv_nsec; // get time in nanoseconds
		float fDeltaSeconds = float( uNowNano - uPreviousTime ) * 0.000000001f; // 1 second = 1,000,000,000 nanoseconds
		uPreviousTime = uNowNano; // set previous time to new time

		// If not paused...
		if ( !m_bIsPaused )
		{
			// Update logic
			Update( fDeltaSeconds );
		}

		// If visible
		if ( m_bIsVisible )
		{
			// Draw
			Draw();
		}

	}

	LOGV( "[Example]: Mainloop terminated." );
}

void ExampleAndroidHandler::Update( float fDeltaSeconds )
{
	// Do stuff here
}

void ExampleAndroidHandler::Draw()
{
	// Draw things here
	glClearColor( 0.0f, 0.0f, 1.0f, 1.0f );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	if ( !eglSwapBuffers( m_Display, m_Surface ) )
	{
		LOGE( "[Example]: eglSwapBuffers() returned error %d", eglGetError() );
	}
}

bool ExampleAndroidHandler::InitOpenGL()
{
	const EGLint attribs[] =
	{
		EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
		EGL_BLUE_SIZE, 8,
		EGL_GREEN_SIZE, 8,
		EGL_RED_SIZE, 8,
		EGL_NONE
	};

	EGLDisplay display;
	EGLConfig config;
	EGLint numConfigs;
	EGLint format;
	EGLSurface surface;
	EGLContext context;
	EGLint width;
	EGLint height;
	GLfloat ratio;

	LOGV( "[Example]: Initializing context" );

	if ( ( display = eglGetDisplay( EGL_DEFAULT_DISPLAY ) ) == EGL_NO_DISPLAY )
	{
		LOGE( "[Example]: eglGetDisplay() returned error %d", eglGetError() );
		return false;
	}

	if ( !eglInitialize( display, 0, 0 ) )
	{
		LOGE( "[Example]: eglInitialize() returned error %d", eglGetError() );
		return false;
	}

	if ( !eglChooseConfig(display, attribs, &config, 1, &numConfigs) )
	{
		LOGE( "[Example]: eglChooseConfig() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format) )
	{
		LOGE( "[Example]: eglGetConfigAttrib() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	// Set buffer geometry using our window which is saved in Android
	ANativeWindow_setBuffersGeometry( Android::GetWindow(), 0, 0, format );

	if ( !( surface = eglCreateWindowSurface( display, config, Android::GetWindow(), 0 ) ) )
	{
		LOGE( "[Example]: eglCreateWindowSurface() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !( context = eglCreateContext( display, config, 0, 0 ) ) )
	{
		LOGE( "[Example]: eglCreateContext() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglMakeCurrent(display, surface, surface, context ) )
	{
		LOGE( "[Example]: eglMakeCurrent() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	if ( !eglQuerySurface( display, surface, EGL_WIDTH, &width ) ||
		!eglQuerySurface( display, surface, EGL_HEIGHT, &height ) )
	{
		LOGE( "[Example]: eglQuerySurface() returned error %d", eglGetError() );
		DestroyOpenGL();
		return false;
	}

	m_Display = display;
	m_Surface = surface;
	m_Context = context;

	glDisable( GL_DITHER );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST );
	glClearColor(0, 0, 0, 0);
	glEnable( GL_CULL_FACE );
	glEnable( GL_DEPTH_TEST );

	return true;
}

void ExampleAndroidHandler::DestroyOpenGL()
{
	if ( m_Display )
	{
		LOGV( "[Example]: Shutting down OpenGL" );

		eglMakeCurrent( m_Display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT );
		eglDestroyContext( m_Display, m_Context );
		eglDestroySurface( m_Display, m_Surface);
		eglTerminate( m_Display );

		m_Display = EGL_NO_DISPLAY;
		m_Surface = EGL_NO_SURFACE;
		m_Context = NULL;
	}
}

// Application
void ExampleAndroidHandler::OnShutdown()
{
	LOGV( "[Example]: Shutting down!" );
	m_bShouldQuit = true;
}

// Surface
void ExampleAndroidHandler::OnSurfaceCreated()
{
	LOGV( "[Example]: Creating surface!" );
	if ( InitOpenGL() == false )
	{
		m_bShouldQuit = true;
	}
}

void ExampleAndroidHandler::OnSurfaceChanged( int iPixelFormat, int iWidth, int iHeight )
{
	LOGV( "[Example]: Setting viewports!" );

	// Set new viewport
	glViewport( 0, 0, iWidth, iHeight );
}

void ExampleAndroidHandler::OnSurfaceDestroyed()
{
	LOGV( "[Example]: Destroying egl!" );
	DestroyOpenGL();
}

// States
void ExampleAndroidHandler::OnPause()
{
	LOGV( "[Example]: Paused!" );
	m_bIsPaused = true;
}

void ExampleAndroidHandler::OnResume()
{
	LOGV( "[Example]: Resumed!" );
	m_bIsPaused = false;
}

void ExampleAndroidHandler::OnVisible()
{
	LOGV( "[Example]: Visible!" );
	m_bIsVisible = true;
}

void ExampleAndroidHandler::OnHidden()
{
	LOGV( "[Example]: Hidden!" );
	m_bIsVisible = false;
}

void ExampleAndroidHandler::OnLowMemory()
{
	LOGV( "[Example]: Clearing some memory to stay alive..." );

	// BigMemoryObject->Release();
}

// Input
void ExampleAndroidHandler::OnKey( int iKeyCode, wchar_t iUnicodeChar )
{
	LOGV( "[Example]: Got key! %i %c", iKeyCode, iUnicodeChar );
}

void ExampleAndroidHandler::OnTouch( int iPointerID, float fPosX, float fPosY, int iAction )
{
	LOGV( "[Example]: Touch: %i, x: %f y:, %f action:, %i.", iPointerID, fPosX, fPosY, iAction );

	if ( iAction == 0 )
	{
		// On touch start show keyboard!
		Android::ShowKeyboard();
	}

	else if ( iAction == 1 )
	{
		// On touch up, hide keyboard...
		Android::HideKeyboard();
	}
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <iostream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/video/video.hpp"

#include <utils.h>

#include <vector>

#include "types.h"

using namespace std;
using namespace visual_odometry;
using namespace visual_odometry::utils;

#define LOG(...) printf(__VA_ARGS__)

//void CalcProjectionMatrix(const vector<cv::Point3f>& pt_3d, const vector<cv::Point2f>& pt_2d, cv::Mat pM);

class Frame {

public:

	Frame(int n) {
		squareFeatures.resize(n);
		triangleFeatures.resize(n);
		projMatrix.create(cv::Size(4, 3), CV_64FC1);
	}

    Frame(const Frame& other) {
        squareFeatures = other.squareFeatures;
        triangleFeatures = other.triangleFeatures;
        projMatrix = other.projMatrix.clone();
        intrinsic = other.intrinsic;
        distortion = other.distortion;
        image = other.image;
        keypoints = other.keypoints;
        descriptors = other.descriptors.clone();
    }

	vector<Feature> squareFeatures;
	vector<Feature> triangleFeatures;
	cv::Mat projMatrix;

    // Intrinsics params
    cv::Mat intrinsic;   // intrinsic parameters
    cv::Mat distortion;  // lens distortion coefficients

	cv::Mat image;

	vector<cv::KeyPoint> keypoints;
	cv::Mat descriptors;

    void calcProjMatrix() {

		vector<cv::Point3f> points3d;
		vector<cv::Point2f> points2d;

		for (int i = 0; i < squareFeatures.size(); i++) {
			points3d.push_back(squareFeatures[i].p3D_);
			points2d.push_back(squareFeatures[i].kp_.pt);
		}

//        bool cv::solvePnPRansac	(	InputArray 	objectPoints,
//        InputArray 	imagePoints,
//        InputArray 	cameraMatrix,
//        InputArray 	distCoeffs,
//        OutputArray 	rvec,
//        OutputArray 	tvec,
//        bool 	useExtrinsicGuess = false,
//        int 	iterationsCount = 100,
//        float 	reprojectionError = 8.0,
//        double 	confidence = 0.99,
//        OutputArray 	inliers = noArray(),
//        int 	flags = SOLVEPNP_ITERATIVE
//        )

        LOG("Number of Points %d\n", points3d.size());

        cv::Mat tvec, rvec;
        cv::solvePnPRansac(points3d, points2d, intrinsic, distortion, rvec, tvec, false, 100, 8.0, 0.99);

        cv::Mat R;
        cv::Rodrigues(rvec, R);

        cv::Mat Pose(3,4, R.type());
        R.copyTo(Pose(cv::Rect(0, 0, 3, 3)));
        tvec.copyTo(Pose(cv::Rect(3, 0, 1, 3)));
        projMatrix = intrinsic*Pose;
        //CalcProjectionMatrix(points3d, points2d, projMatrix);

		printProjMatrix();
	}

    void detectAndDescribe() {
		char algorithm[] = "SIFT";
		cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create(algorithm);
		cv::Ptr<cv::DescriptorExtractor> descriptor = cv::DescriptorExtractor::create(algorithm);

		keypoints.clear();

		detector->detect(image, keypoints);
		descriptor->compute(image, keypoints, descriptors);
	}

    void loadIntrinsicsFromFile(const std::string& filename){

        cv::FileStorage cvfs(filename.c_str(),CV_STORAGE_READ);
        if( cvfs.isOpened() ){
           cvfs["mat_intrinsicMat"] >> intrinsic;
           cvfs["mat_distortionMat"] >> distortion;
        }
    }

    void loadKpFromFile(const std::string& filename){

		FILE *f = fopen(filename.c_str(), "r");
		float x, y;
		int index = 0;

		while (fscanf(f, "%f,%f", &x, &y) == 2){
			squareFeatures[index].kp_ = cv::KeyPoint(x, y, 1);
			squareFeatures[index].type_ = Feature::SQUARE;
			index++;
		}

		fclose(f);

	}

    void load3DPointsFromFile(const std::string& filename){

		FILE *f = fopen(filename.c_str(), "r");
		cv::Point3f p;
		int index = 0;

		while (fscanf(f, "%f,%f,%f", &p.x, &p.y, &p.z) == 3){
			squareFeatures[index].p3D_ = p;
			index++;
		}

		fclose(f);

	}

    void printProjMatrix() {
		static int projIndex = 0;
        LOG("Proj Matrix #%d:", projIndex++);
		for (int i = 0; i < 12; i++) {
            LOG("%s%lf\t", (i % 4 == 0) ? "\n" : "", projMatrix.at<double>(i / 4, i % 4));
		}
        LOG("\n");

		projectAndShow();
	}

    void readNextFrame() {

		static int findex = 0;
		char buf[256];
		sprintf(buf, "S01L03_VGA/S01L03_VGA_%04d.png", findex++);

		image = cv::imread(buf);
		if (image.empty()) {
			exit(0);
		}
	}

    void updateUsingKLT(Frame& previousFrame) {

		squareFeatures.clear();

		vector<cv::Point2f> previousPoints;
		vector<cv::Point2f> currentPoints;
		vector<uchar> status;
		cv::Mat errors;
		for (int i = 0; i < previousFrame.squareFeatures.size(); i++) {
			previousPoints.push_back(previousFrame.squareFeatures[i].kp_.pt);
		}

		cv::calcOpticalFlowPyrLK(previousFrame.image, image, previousPoints, currentPoints, status, errors);

		for (int i = 0; i < previousPoints.size(); i++) {
			if (status[i] != 0 /*&& errors.at<float>(i) < 4*/) { // klt ok!
				Feature f;
				f.kp_ = cv::KeyPoint(currentPoints[i].x, currentPoints[i].y, 1);
				f.p3D_ = previousFrame.squareFeatures[i].p3D_;
				squareFeatures.push_back(f);
			}
		}
	}

    void projectAndShow() {

		cv::Mat point3D;
		point3D.create(cv::Size(1, 4), CV_64FC1);
		point3D.at<double>(0) = 140;
		point3D.at<double>(1) = -264;
		point3D.at<double>(2) = 300;
		point3D.at<double>(3) = 1;

		cv::Mat result;
		result.create(cv::Size(1, 3), CV_64FC1);
		cv::gemm(projMatrix, point3D, 1, 0, 0, result);

		//printf("%lf %lf %lf\n", result.at<double>(0), result.at<double>(1), result.at<double>(2));
		//printf("%lf %lf\n", result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2));

		cv::Mat imcopy = image.clone();
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)),4,cv::Scalar(0,0,255),-1);
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 3, cv::Scalar(255, 255, 255), -1);
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 2, cv::Scalar(0, 0, 255), -1);

        for(int i = 0; i < squareFeatures.size(); i++){

            point3D.at<double>(0) = squareFeatures[i].p3D_.x;
            point3D.at<double>(1) = squareFeatures[i].p3D_.y;
            point3D.at<double>(2) = squareFeatures[i].p3D_.z;
            point3D.at<double>(3) = 1;

            cv::gemm(projMatrix,  point3D , 1, 0, 0, result);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)),4,cv::Scalar(0,0,255),-1);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 3, cv::Scalar(255, 255, 255), -1);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 2, cv::Scalar(0, 0, 255), -1);

        }

        for(int i = 0; i < triangleFeatures.size(); i++){
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y), 4,cv::Scalar(0,125,255),-1);
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y), 3, cv::Scalar(125, 255, 255), -1);
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y) , 2, cv::Scalar(125, 125, 255), -1);
        }

        cv::imshow("frame", imcopy);
		if (cv::waitKey(0) == 27) exit(0);
	}

};

void matchAndTriangulate(Frame& previousFrame, Frame& currentFrame, cv::Mat intrinsics, cv::Mat distortion) {

	static GenericMatcher matcher;
	vector<cv::DMatch> matches;

	matcher.match(currentFrame.descriptors, previousFrame.descriptors, matches, currentFrame.keypoints, previousFrame.keypoints);

	vector<cv::Point2f> previousTriangulate, currentTriangulate;
	cv::Mat	outputTriangulate;
	outputTriangulate.create(cv::Size(4, matches.size()), CV_32FC1);

	for (int i = 0; i < matches.size(); i++) {
        cv::Point pt1 = previousFrame.keypoints[matches[i].trainIdx].pt;
        cv::Point pt2 = currentFrame.keypoints[matches[i].queryIdx].pt;
		previousTriangulate.push_back(pt1);
		currentTriangulate.push_back(pt2);
	}

    // undistort
    std::vector<cv::Point2f> previousTriangulateUnd, currentTriangulateUnd;
    cv::undistortPoints(previousTriangulate, previousTriangulateUnd, intrinsics, distortion);
    cv::undistortPoints(currentTriangulate, currentTriangulateUnd, intrinsics, distortion);

    cv::triangulatePoints(intrinsics.inv()*previousFrame.projMatrix, intrinsics.inv()*currentFrame.projMatrix, previousTriangulateUnd, currentTriangulateUnd, outputTriangulate);

	for (int i = 0; i < matches.size(); i++) {
		Feature f;
		f.kp_ = cv::KeyPoint(currentTriangulate[i].x, currentTriangulate[i].y, 1);
		f.p3D_ = cv::Point3f(outputTriangulate.at<float>(0, i) / outputTriangulate.at<float>(3, i),
			outputTriangulate.at<float>(1, i) / outputTriangulate.at<float>(3, i),
			outputTriangulate.at<float>(2, i) / outputTriangulate.at<float>(3, i));
		currentFrame.squareFeatures.push_back(f);
	}
}

bool has_enough_baseline(cv::Mat pose1, cv::Mat pose2, double thr_baseline){

    cv::Mat R1 = pose1(cv::Range::all(), cv::Range(0, 3));
    cv::Mat T1 = pose1(cv::Range::all(), cv::Range(3, 4));

    cv::Mat R2 = pose2(cv::Range::all(), cv::Range(0, 3));
    cv::Mat T2 = pose2(cv::Range::all(), cv::Range(3, 4));

    cv::Mat pos1, pos2;
    pos1 = -R1.inv() * T1;
    pos2 = -R2.inv() * T2;

    double baseline = cv::norm(pos1 - pos2);

    LOG("Baseline %f\n", baseline);
    return baseline >= thr_baseline;
}

int main() {

    //cvNamedWindow("frame", CV_WINDOW_NORMAL);
    //cvSetWindowProperty("frame", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);

    Frame previousFrame(40);
    Frame currentFrame(40);
    Frame keyFrame(40);

	cv::initModule_nonfree();

	previousFrame.loadKpFromFile("S01_2Ddata_dst_init.csv");
	previousFrame.load3DPointsFromFile("S01_3Ddata_dst_init.csv");
    previousFrame.loadIntrinsicsFromFile("intrinsics.xml");
    currentFrame.loadIntrinsicsFromFile("intrinsics.xml");

	previousFrame.readNextFrame();

	previousFrame.calcProjMatrix();

    previousFrame.detectAndDescribe();

	keyFrame = previousFrame;

	int counter = 1;

	while (true) {

		currentFrame.readNextFrame();

		currentFrame.updateUsingKLT(previousFrame);

		currentFrame.calcProjMatrix();

        bool enough_baseline = has_enough_baseline(keyFrame.intrinsic.inv()*keyFrame.projMatrix, currentFrame.intrinsic.inv()*currentFrame.projMatrix, 150);

        if (enough_baseline /*counter % 20 == 0*/) { // keyframe interval // change here from 5 to any other number
			currentFrame.detectAndDescribe();

            LOG("BEFORE: %d\t", currentFrame.squareFeatures.size());
            matchAndTriangulate(keyFrame, currentFrame, currentFrame.intrinsic, currentFrame.distortion);
            LOG("AFTER: %d\n", currentFrame.squareFeatures.size());

			keyFrame = currentFrame;
            // This is extremely important (otherwise all Frames will have a common projMatrix in the memory)
            keyFrame.projMatrix = currentFrame.projMatrix.clone();
		}

		previousFrame = currentFrame;
		counter++;
	}

}
<commit_msg>fixed triangulation assert error and optimised params<commit_after>#include <stdio.h>
#include <iostream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/video/video.hpp"

#include <utils.h>

#include <vector>

#include "types.h"

#include <algorithm>

using namespace std;
using namespace visual_odometry;
using namespace visual_odometry::utils;

#define LOG(...) printf(__VA_ARGS__)

//void CalcProjectionMatrix(const vector<cv::Point3f>& pt_3d, const vector<cv::Point2f>& pt_2d, cv::Mat pM);

class Frame {

public:

	Frame(int n) {
		squareFeatures.resize(n);
		triangleFeatures.resize(n);
		projMatrix.create(cv::Size(4, 3), CV_64FC1);
	}

    Frame(const Frame& other) {
        squareFeatures = other.squareFeatures;
        triangleFeatures = other.triangleFeatures;
        projMatrix = other.projMatrix.clone();
        intrinsic = other.intrinsic;
        distortion = other.distortion;
        image = other.image;
        keypoints = other.keypoints;
        descriptors = other.descriptors.clone();
    }

	vector<Feature> squareFeatures;
	vector<Feature> triangleFeatures;
	cv::Mat projMatrix;

    // Intrinsics params
    cv::Mat intrinsic;   // intrinsic parameters
    cv::Mat distortion;  // lens distortion coefficients

	cv::Mat image;

	vector<cv::KeyPoint> keypoints;
	cv::Mat descriptors;

    void calcProjMatrix() {

		vector<cv::Point3f> points3d;
		vector<cv::Point2f> points2d;

        for (int i = std::max<int>(0,(squareFeatures.size()-100)); i < squareFeatures.size(); i++) {
			points3d.push_back(squareFeatures[i].p3D_);
			points2d.push_back(squareFeatures[i].kp_.pt);
		}

//        bool cv::solvePnPRansac	(	InputArray 	objectPoints,
//        InputArray 	imagePoints,
//        InputArray 	cameraMatrix,
//        InputArray 	distCoeffs,
//        OutputArray 	rvec,
//        OutputArray 	tvec,
//        bool 	useExtrinsicGuess = false,
//        int 	iterationsCount = 100,
//        float 	reprojectionError = 8.0,
//        double 	confidence = 0.99,
//        OutputArray 	inliers = noArray(),
//        int 	flags = SOLVEPNP_ITERATIVE
//        )

        LOG("Number of Points %d\n", points3d.size());

        cv::Mat tvec, rvec;
        cv::solvePnPRansac(points3d, points2d, intrinsic, distortion, rvec, tvec, false, 300, 8.0, 0.99);

        cv::Mat R;
        cv::Rodrigues(rvec, R);

        cv::Mat Pose(3,4, R.type());
        R.copyTo(Pose(cv::Rect(0, 0, 3, 3)));
        tvec.copyTo(Pose(cv::Rect(3, 0, 1, 3)));
        projMatrix = intrinsic*Pose;
        //CalcProjectionMatrix(points3d, points2d, projMatrix);

		printProjMatrix();
	}

    void detectAndDescribe() {
		char algorithm[] = "SIFT";
		cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create(algorithm);
		cv::Ptr<cv::DescriptorExtractor> descriptor = cv::DescriptorExtractor::create(algorithm);

		keypoints.clear();

		detector->detect(image, keypoints);
		descriptor->compute(image, keypoints, descriptors);
	}

    void loadIntrinsicsFromFile(const std::string& filename){

        cv::FileStorage cvfs(filename.c_str(),CV_STORAGE_READ);
        if( cvfs.isOpened() ){
           cvfs["mat_intrinsicMat"] >> intrinsic;
           cvfs["mat_distortionMat"] >> distortion;
        }
    }

    void loadKpFromFile(const std::string& filename){

		FILE *f = fopen(filename.c_str(), "r");
		float x, y;
		int index = 0;

		while (fscanf(f, "%f,%f", &x, &y) == 2){
			squareFeatures[index].kp_ = cv::KeyPoint(x, y, 1);
			squareFeatures[index].type_ = Feature::SQUARE;
			index++;
		}

		fclose(f);

	}

    void load3DPointsFromFile(const std::string& filename){

		FILE *f = fopen(filename.c_str(), "r");
		cv::Point3f p;
		int index = 0;

		while (fscanf(f, "%f,%f,%f", &p.x, &p.y, &p.z) == 3){
			squareFeatures[index].p3D_ = p;
			index++;
		}

		fclose(f);

	}

    void printProjMatrix() {
		static int projIndex = 0;
        LOG("Proj Matrix #%d:", projIndex++);
		for (int i = 0; i < 12; i++) {
            LOG("%s%lf\t", (i % 4 == 0) ? "\n" : "", projMatrix.at<double>(i / 4, i % 4));
		}
        LOG("\n");

		projectAndShow();
	}

    void readNextFrame() {

		static int findex = 0;
		char buf[256];
		sprintf(buf, "S01L03_VGA/S01L03_VGA_%04d.png", findex++);

		image = cv::imread(buf);
		if (image.empty()) {
			exit(0);
		}
	}

    void updateUsingKLT(Frame& previousFrame) {

		squareFeatures.clear();

		vector<cv::Point2f> previousPoints;
		vector<cv::Point2f> currentPoints;
		vector<uchar> status;
		cv::Mat errors;
		for (int i = 0; i < previousFrame.squareFeatures.size(); i++) {
			previousPoints.push_back(previousFrame.squareFeatures[i].kp_.pt);
		}

		cv::calcOpticalFlowPyrLK(previousFrame.image, image, previousPoints, currentPoints, status, errors);

		for (int i = 0; i < previousPoints.size(); i++) {
			if (status[i] != 0 /*&& errors.at<float>(i) < 4*/) { // klt ok!
				Feature f;
				f.kp_ = cv::KeyPoint(currentPoints[i].x, currentPoints[i].y, 1);
				f.p3D_ = previousFrame.squareFeatures[i].p3D_;
				squareFeatures.push_back(f);
			}
		}
	}

    void projectAndShow() {

		cv::Mat point3D;
		point3D.create(cv::Size(1, 4), CV_64FC1);
		point3D.at<double>(0) = 140;
		point3D.at<double>(1) = -264;
		point3D.at<double>(2) = 300;
		point3D.at<double>(3) = 1;

		cv::Mat result;
		result.create(cv::Size(1, 3), CV_64FC1);
		cv::gemm(projMatrix, point3D, 1, 0, 0, result);

		//printf("%lf %lf %lf\n", result.at<double>(0), result.at<double>(1), result.at<double>(2));
		//printf("%lf %lf\n", result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2));

		cv::Mat imcopy = image.clone();
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)),4,cv::Scalar(0,0,255),-1);
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 3, cv::Scalar(255, 255, 255), -1);
		cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 2, cv::Scalar(0, 0, 255), -1);

        for(int i = 0; i < squareFeatures.size(); i++){

            point3D.at<double>(0) = squareFeatures[i].p3D_.x;
            point3D.at<double>(1) = squareFeatures[i].p3D_.y;
            point3D.at<double>(2) = squareFeatures[i].p3D_.z;
            point3D.at<double>(3) = 1;

            cv::gemm(projMatrix,  point3D , 1, 0, 0, result);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)),4,cv::Scalar(0,0,255),-1);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 3, cv::Scalar(255, 255, 255), -1);
            cv::circle(imcopy, cv::Point(result.at<double>(0) / result.at<double>(2), result.at<double>(1) / result.at<double>(2)), 2, cv::Scalar(0, 0, 255), -1);

        }

        for(int i = 0; i < triangleFeatures.size(); i++){
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y), 4,cv::Scalar(0,125,255),-1);
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y), 3, cv::Scalar(125, 255, 255), -1);
            cv::circle(imcopy, cv::Point(triangleFeatures[i].kp_.pt.x,triangleFeatures[i].kp_.pt.y) , 2, cv::Scalar(125, 125, 255), -1);
        }

        cv::imshow("frame", imcopy);
		if (cv::waitKey(0) == 27) exit(0);
	}

};

void matchAndTriangulate(Frame& previousFrame, Frame& currentFrame, cv::Mat intrinsics, cv::Mat distortion) {

	static GenericMatcher matcher;
	vector<cv::DMatch> matches;

	matcher.match(currentFrame.descriptors, previousFrame.descriptors, matches, currentFrame.keypoints, previousFrame.keypoints);

    LOG("NMatches %d\n",matches.size());

	vector<cv::Point2f> previousTriangulate, currentTriangulate;
	cv::Mat	outputTriangulate;
	outputTriangulate.create(cv::Size(4, matches.size()), CV_32FC1);

	for (int i = 0; i < matches.size(); i++) {
        cv::Point pt1 = previousFrame.keypoints[matches[i].trainIdx].pt;
        cv::Point pt2 = currentFrame.keypoints[matches[i].queryIdx].pt;
		previousTriangulate.push_back(pt1);
		currentTriangulate.push_back(pt2);
	}

    if( previousTriangulate.size() == 0 || currentTriangulate.size() == 0 ){
        //LOG("Triangulation Points %d %d\n",previousTriangulate.size(),currentTriangulate.size());
        return;
    }

    // undistort
    std::vector<cv::Point2f> previousTriangulateUnd, currentTriangulateUnd;
    cv::undistortPoints(previousTriangulate, previousTriangulateUnd, intrinsics, distortion);
    cv::undistortPoints(currentTriangulate, currentTriangulateUnd, intrinsics, distortion);

    cv::triangulatePoints(intrinsics.inv()*previousFrame.projMatrix, intrinsics.inv()*currentFrame.projMatrix, previousTriangulateUnd, currentTriangulateUnd, outputTriangulate);

	for (int i = 0; i < matches.size(); i++) {
		Feature f;
		f.kp_ = cv::KeyPoint(currentTriangulate[i].x, currentTriangulate[i].y, 1);
		f.p3D_ = cv::Point3f(outputTriangulate.at<float>(0, i) / outputTriangulate.at<float>(3, i),
			outputTriangulate.at<float>(1, i) / outputTriangulate.at<float>(3, i),
			outputTriangulate.at<float>(2, i) / outputTriangulate.at<float>(3, i));
		currentFrame.squareFeatures.push_back(f);
	}
}

bool has_enough_baseline(cv::Mat pose1, cv::Mat pose2, double thr_baseline){

    cv::Mat R1 = pose1(cv::Range::all(), cv::Range(0, 3));
    cv::Mat T1 = pose1(cv::Range::all(), cv::Range(3, 4));

    cv::Mat R2 = pose2(cv::Range::all(), cv::Range(0, 3));
    cv::Mat T2 = pose2(cv::Range::all(), cv::Range(3, 4));

    cv::Mat pos1, pos2;
    pos1 = -R1.inv() * T1;
    pos2 = -R2.inv() * T2;

    double baseline = cv::norm(pos1 - pos2);

    LOG("Baseline %f\n", baseline);
    return baseline >= thr_baseline;
}

int main() {

    //cvNamedWindow("frame", CV_WINDOW_NORMAL);
    //cvSetWindowProperty("frame", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);

    Frame previousFrame(40);
    Frame currentFrame(40);
    Frame keyFrame(40);

	cv::initModule_nonfree();

	previousFrame.loadKpFromFile("S01_2Ddata_dst_init.csv");
	previousFrame.load3DPointsFromFile("S01_3Ddata_dst_init.csv");
    previousFrame.loadIntrinsicsFromFile("intrinsics.xml");
    currentFrame.loadIntrinsicsFromFile("intrinsics.xml");

	previousFrame.readNextFrame();

	previousFrame.calcProjMatrix();

    previousFrame.detectAndDescribe();

	keyFrame = previousFrame;

	int counter = 1;

	while (true) {

		currentFrame.readNextFrame();

		currentFrame.updateUsingKLT(previousFrame);

		currentFrame.calcProjMatrix();

        bool enough_baseline = has_enough_baseline(keyFrame.intrinsic.inv()*keyFrame.projMatrix, currentFrame.intrinsic.inv()*currentFrame.projMatrix, 150);

        if (enough_baseline /*counter % 20 == 0*/) { // keyframe interval // change here from 5 to any other number
			currentFrame.detectAndDescribe();

            LOG("BEFORE: %d\t", currentFrame.squareFeatures.size());
            matchAndTriangulate(keyFrame, currentFrame, currentFrame.intrinsic, currentFrame.distortion);
            LOG("AFTER: %d\n", currentFrame.squareFeatures.size());

			keyFrame = currentFrame;
            // This is extremely important (otherwise all Frames will have a common projMatrix in the memory)
            keyFrame.projMatrix = currentFrame.projMatrix.clone();
		}

		previousFrame = currentFrame;
		counter++;
	}

}
<|endoftext|>


<commit_before>/*
 * Copyright 2014 Facebook, Inc.
 *
 * 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.
 */

#include "folly/Subprocess.h"

#include <unistd.h>
#include <sys/types.h>
#include <dirent.h>

#include <boost/container/flat_set.hpp>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "folly/Exception.h"
#include "folly/Format.h"
#include "folly/FileUtil.h"
#include "folly/String.h"
#include "folly/gen/Base.h"
#include "folly/gen/File.h"
#include "folly/gen/String.h"
#include "folly/experimental/io/FsUtil.h"

using namespace folly;

TEST(SimpleSubprocessTest, ExitsSuccessfully) {
  Subprocess proc(std::vector<std::string>{ "/bin/true" });
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ExitsSuccessfullyChecked) {
  Subprocess proc(std::vector<std::string>{ "/bin/true" });
  proc.waitChecked();
}

TEST(SimpleSubprocessTest, ExitsWithError) {
  Subprocess proc(std::vector<std::string>{ "/bin/false" });
  EXPECT_EQ(1, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ExitsWithErrorChecked) {
  Subprocess proc(std::vector<std::string>{ "/bin/false" });
  EXPECT_THROW(proc.waitChecked(), CalledProcessError);
}

#define EXPECT_SPAWN_ERROR(err, errMsg, cmd, ...) \
  do { \
    try { \
      Subprocess proc(std::vector<std::string>{ (cmd), ## __VA_ARGS__ }); \
      ADD_FAILURE() << "expected an error when running " << (cmd); \
    } catch (const SubprocessSpawnError& ex) { \
      EXPECT_EQ((err), ex.errnoValue()); \
      if (StringPiece(ex.what()).find(errMsg) == StringPiece::npos) { \
        ADD_FAILURE() << "failed to find \"" << (errMsg) << \
          "\" in exception: \"" << ex.what() << "\""; \
      } \
    } \
  } while (0)

TEST(SimpleSubprocessTest, ExecFails) {
  EXPECT_SPAWN_ERROR(ENOENT, "failed to execute /no/such/file:",
                     "/no/such/file");
  EXPECT_SPAWN_ERROR(EACCES, "failed to execute /etc/passwd:",
                     "/etc/passwd");
  EXPECT_SPAWN_ERROR(ENOTDIR, "failed to execute /etc/passwd/not/a/file:",
                     "/etc/passwd/not/a/file");
}

TEST(SimpleSubprocessTest, ShellExitsSuccesssfully) {
  Subprocess proc("true");
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ShellExitsWithError) {
  Subprocess proc("false");
  EXPECT_EQ(1, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ChangeChildDirectorySuccessfully) {
  // The filesystem root normally lacks a 'true' binary
  EXPECT_EQ(0, chdir("/"));
  EXPECT_SPAWN_ERROR(ENOENT, "failed to execute ./true", "./true");
  // The child can fix that by moving to /bin before exec().
  Subprocess proc("./true", Subprocess::Options().chdir("/bin"));
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ChangeChildDirectoryWithError) {
  try {
    Subprocess proc(
      std::vector<std::string>{"/bin/true"},
      Subprocess::Options().chdir("/usually/this/is/not/a/valid/directory/")
    );
    ADD_FAILURE() << "expected to fail when changing the child's directory";
  } catch (const SubprocessSpawnError& ex) {
    EXPECT_EQ(ENOENT, ex.errnoValue());
    const std::string expectedError =
      "error preparing to execute /bin/true: No such file or directory";
    if (StringPiece(ex.what()).find(expectedError) == StringPiece::npos) {
      ADD_FAILURE() << "failed to find \"" << expectedError <<
        "\" in exception: \"" << ex.what() << "\"";
    }
  }
}

namespace {
boost::container::flat_set<int> getOpenFds() {
  auto pid = getpid();
  auto dirname = to<std::string>("/proc/", pid, "/fd");

  boost::container::flat_set<int> fds;
  for (fs::directory_iterator it(dirname);
       it != fs::directory_iterator();
       ++it) {
    int fd = to<int>(it->path().filename().native());
    fds.insert(fd);
  }
  return fds;
}

template<class Runnable>
void checkFdLeak(const Runnable& r) {
  // Get the currently open fds.  Check that they are the same both before and
  // after calling the specified function.  We read the open fds from /proc.
  // (If we wanted to work even on systems that don't have /proc, we could
  // perhaps create and immediately close a socket both before and after
  // running the function, and make sure we got the same fd number both times.)
  auto fdsBefore = getOpenFds();
  r();
  auto fdsAfter = getOpenFds();
  EXPECT_EQ(fdsAfter.size(), fdsBefore.size());
}
}

// Make sure Subprocess doesn't leak any file descriptors
TEST(SimpleSubprocessTest, FdLeakTest) {
  // Normal execution
  checkFdLeak([] {
    Subprocess proc("true");
    EXPECT_EQ(0, proc.wait().exitStatus());
  });
  // Normal execution with pipes
  checkFdLeak([] {
    Subprocess proc("echo foo; echo bar >&2",
                    Subprocess::pipeStdout() | Subprocess::pipeStderr());
    auto p = proc.communicate();
    EXPECT_EQ("foo\n", p.first);
    EXPECT_EQ("bar\n", p.second);
    proc.waitChecked();
  });

  // Test where the exec call fails()
  checkFdLeak([] {
    EXPECT_SPAWN_ERROR(ENOENT, "failed to execute", "/no/such/file");
  });
  // Test where the exec call fails() with pipes
  checkFdLeak([] {
    try {
      Subprocess proc(std::vector<std::string>({"/no/such/file"}),
                      Subprocess::pipeStdout().stderr(Subprocess::PIPE));
      ADD_FAILURE() << "expected an error when running /no/such/file";
    } catch (const SubprocessSpawnError& ex) {
      EXPECT_EQ(ENOENT, ex.errnoValue());
    }
  });
}

TEST(ParentDeathSubprocessTest, ParentDeathSignal) {
  // Find out where we are.
  static constexpr size_t pathLength = 2048;
  char buf[pathLength + 1];
  int r = readlink("/proc/self/exe", buf, pathLength);
  CHECK_ERR(r);
  buf[r] = '\0';

  fs::path helper(buf);
  helper.remove_filename();
  helper /= "subprocess_test_parent_death_helper";

  fs::path tempFile(fs::temp_directory_path() / fs::unique_path());

  std::vector<std::string> args {helper.string(), tempFile.string()};
  Subprocess proc(args);
  // The helper gets killed by its child, see details in
  // SubprocessTestParentDeathHelper.cpp
  ASSERT_EQ(SIGKILL, proc.wait().killSignal());

  // Now wait for the file to be created, see details in
  // SubprocessTestParentDeathHelper.cpp
  while (!fs::exists(tempFile)) {
    usleep(20000);  // 20ms
  }

  fs::remove(tempFile);
}

TEST(PopenSubprocessTest, PopenRead) {
  Subprocess proc("ls /", Subprocess::pipeStdout());
  int found = 0;
  gen::byLine(File(proc.stdout())) |
    [&] (StringPiece line) {
      if (line == "etc" || line == "bin" || line == "usr") {
        ++found;
      }
    };
  EXPECT_EQ(3, found);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, SimpleRead) {
  Subprocess proc(std::vector<std::string>{ "/bin/echo", "-n", "foo", "bar"},
                  Subprocess::pipeStdout());
  auto p = proc.communicate();
  EXPECT_EQ("foo bar", p.first);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, BigWrite) {
  const int numLines = 1 << 20;
  std::string line("hello\n");
  std::string data;
  data.reserve(numLines * line.size());
  for (int i = 0; i < numLines; ++i) {
    data.append(line);
  }

  Subprocess proc("wc -l", Subprocess::pipeStdin() | Subprocess::pipeStdout());
  auto p = proc.communicate(data);
  EXPECT_EQ(folly::format("{}\n", numLines).str(), p.first);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, Duplex) {
  // Take 10MB of data and pass them through a filter.
  // One line, as tr is line-buffered
  const int bytes = 10 << 20;
  std::string line(bytes, 'x');

  Subprocess proc("tr a-z A-Z",
                  Subprocess::pipeStdin() | Subprocess::pipeStdout());
  auto p = proc.communicate(line);
  EXPECT_EQ(bytes, p.first.size());
  EXPECT_EQ(std::string::npos, p.first.find_first_not_of('X'));
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, Duplex2) {
  checkFdLeak([] {
    // Pipe 200,000 lines through sed
    const size_t numCopies = 100000;
    auto iobuf = IOBuf::copyBuffer("this is a test\nanother line\n");
    IOBufQueue input;
    for (int n = 0; n < numCopies; ++n) {
      input.append(iobuf->clone());
    }

    std::vector<std::string> cmd({
      "sed", "-u",
      "-e", "s/a test/a successful test/",
      "-e", "/^another line/w/dev/stderr",
    });
    auto options = Subprocess::pipeStdin().pipeStdout().pipeStderr().usePath();
    Subprocess proc(cmd, options);
    auto out = proc.communicateIOBuf(std::move(input));
    proc.waitChecked();

    // Convert stdout and stderr to strings so we can call split() on them.
    fbstring stdoutStr;
    if (out.first.front()) {
      stdoutStr = out.first.move()->moveToFbString();
    }
    fbstring stderrStr;
    if (out.second.front()) {
      stderrStr = out.second.move()->moveToFbString();
    }

    // stdout should be a copy of stdin, with "a test" replaced by
    // "a successful test"
    std::vector<StringPiece> stdoutLines;
    split('\n', stdoutStr, stdoutLines);
    EXPECT_EQ(numCopies * 2 + 1, stdoutLines.size());
    // Strip off the trailing empty line
    if (!stdoutLines.empty()) {
      EXPECT_EQ("", stdoutLines.back());
      stdoutLines.pop_back();
    }
    size_t linenum = 0;
    for (const auto& line : stdoutLines) {
      if ((linenum & 1) == 0) {
        EXPECT_EQ("this is a successful test", line);
      } else {
        EXPECT_EQ("another line", line);
      }
      ++linenum;
    }

    // stderr should only contain the lines containing "another line"
    std::vector<StringPiece> stderrLines;
    split('\n', stderrStr, stderrLines);
    EXPECT_EQ(numCopies + 1, stderrLines.size());
    // Strip off the trailing empty line
    if (!stderrLines.empty()) {
      EXPECT_EQ("", stderrLines.back());
      stderrLines.pop_back();
    }
    for (const auto& line : stderrLines) {
      EXPECT_EQ("another line", line);
    }
  });
}

namespace {

bool readToString(int fd, std::string& buf, size_t maxSize) {
  size_t bytesRead = 0;

  buf.resize(maxSize);
  char* dest = &buf.front();
  size_t remaining = maxSize;

  ssize_t n = -1;
  while (remaining) {
    n = ::read(fd, dest, remaining);
    if (n == -1) {
      if (errno == EINTR) {
        continue;
      }
      if (errno == EAGAIN) {
        break;
      }
      PCHECK("read failed");
    } else if (n == 0) {
      break;
    }
    dest += n;
    remaining -= n;
  }

  buf.resize(dest - buf.data());
  return (n == 0);
}

}  // namespace

TEST(CommunicateSubprocessTest, Chatty) {
  checkFdLeak([] {
    const int lineCount = 1000;

    int wcount = 0;
    int rcount = 0;

    auto options = Subprocess::pipeStdin().pipeStdout().pipeStderr().usePath();
    std::vector<std::string> cmd {
      "sed",
      "-u",
      "-e",
      "s/a test/a successful test/",
    };

    Subprocess proc(cmd, options);

    auto writeCallback = [&] (int pfd, int cfd) -> bool {
      EXPECT_EQ(0, cfd);  // child stdin
      EXPECT_EQ(rcount, wcount);  // chatty, one read for every write

      auto msg = folly::to<std::string>("a test ", wcount, "\n");

      // Not entirely kosher, we should handle partial writes, but this is
      // fine for writes <= PIPE_BUF
      EXPECT_EQ(msg.size(), writeFull(pfd, msg.data(), msg.size()));

      ++wcount;
      proc.enableNotifications(0, false);

      return (wcount == lineCount);
    };

    auto readCallback = [&] (int pfd, int cfd) -> bool {
      EXPECT_EQ(1, cfd);  // child stdout
      EXPECT_EQ(wcount, rcount + 1);

      auto expected =
        folly::to<std::string>("a successful test ", rcount, "\n");

      std::string lineBuf;

      // Not entirely kosher, we should handle partial reads, but this is
      // fine for reads <= PIPE_BUF
      bool r = readToString(pfd, lineBuf, expected.size() + 1);

      EXPECT_EQ((rcount == lineCount), r);  // EOF iff at lineCount
      EXPECT_EQ(expected, lineBuf);

      ++rcount;
      if (rcount != lineCount) {
        proc.enableNotifications(0, true);
      }

      return (rcount == lineCount);
    };

    proc.communicate(readCallback, writeCallback);

    EXPECT_EQ(lineCount, wcount);
    EXPECT_EQ(lineCount, rcount);

    EXPECT_EQ(0, proc.wait().exitStatus());
  });
}
<commit_msg>Fix apparent race in SubprocessTest<commit_after>/*
 * Copyright 2014 Facebook, Inc.
 *
 * 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.
 */

#include "folly/Subprocess.h"

#include <unistd.h>
#include <sys/types.h>
#include <dirent.h>

#include <boost/container/flat_set.hpp>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "folly/Exception.h"
#include "folly/Format.h"
#include "folly/FileUtil.h"
#include "folly/String.h"
#include "folly/gen/Base.h"
#include "folly/gen/File.h"
#include "folly/gen/String.h"
#include "folly/experimental/io/FsUtil.h"

using namespace folly;

TEST(SimpleSubprocessTest, ExitsSuccessfully) {
  Subprocess proc(std::vector<std::string>{ "/bin/true" });
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ExitsSuccessfullyChecked) {
  Subprocess proc(std::vector<std::string>{ "/bin/true" });
  proc.waitChecked();
}

TEST(SimpleSubprocessTest, ExitsWithError) {
  Subprocess proc(std::vector<std::string>{ "/bin/false" });
  EXPECT_EQ(1, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ExitsWithErrorChecked) {
  Subprocess proc(std::vector<std::string>{ "/bin/false" });
  EXPECT_THROW(proc.waitChecked(), CalledProcessError);
}

#define EXPECT_SPAWN_ERROR(err, errMsg, cmd, ...) \
  do { \
    try { \
      Subprocess proc(std::vector<std::string>{ (cmd), ## __VA_ARGS__ }); \
      ADD_FAILURE() << "expected an error when running " << (cmd); \
    } catch (const SubprocessSpawnError& ex) { \
      EXPECT_EQ((err), ex.errnoValue()); \
      if (StringPiece(ex.what()).find(errMsg) == StringPiece::npos) { \
        ADD_FAILURE() << "failed to find \"" << (errMsg) << \
          "\" in exception: \"" << ex.what() << "\""; \
      } \
    } \
  } while (0)

TEST(SimpleSubprocessTest, ExecFails) {
  EXPECT_SPAWN_ERROR(ENOENT, "failed to execute /no/such/file:",
                     "/no/such/file");
  EXPECT_SPAWN_ERROR(EACCES, "failed to execute /etc/passwd:",
                     "/etc/passwd");
  EXPECT_SPAWN_ERROR(ENOTDIR, "failed to execute /etc/passwd/not/a/file:",
                     "/etc/passwd/not/a/file");
}

TEST(SimpleSubprocessTest, ShellExitsSuccesssfully) {
  Subprocess proc("true");
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ShellExitsWithError) {
  Subprocess proc("false");
  EXPECT_EQ(1, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ChangeChildDirectorySuccessfully) {
  // The filesystem root normally lacks a 'true' binary
  EXPECT_EQ(0, chdir("/"));
  EXPECT_SPAWN_ERROR(ENOENT, "failed to execute ./true", "./true");
  // The child can fix that by moving to /bin before exec().
  Subprocess proc("./true", Subprocess::Options().chdir("/bin"));
  EXPECT_EQ(0, proc.wait().exitStatus());
}

TEST(SimpleSubprocessTest, ChangeChildDirectoryWithError) {
  try {
    Subprocess proc(
      std::vector<std::string>{"/bin/true"},
      Subprocess::Options().chdir("/usually/this/is/not/a/valid/directory/")
    );
    ADD_FAILURE() << "expected to fail when changing the child's directory";
  } catch (const SubprocessSpawnError& ex) {
    EXPECT_EQ(ENOENT, ex.errnoValue());
    const std::string expectedError =
      "error preparing to execute /bin/true: No such file or directory";
    if (StringPiece(ex.what()).find(expectedError) == StringPiece::npos) {
      ADD_FAILURE() << "failed to find \"" << expectedError <<
        "\" in exception: \"" << ex.what() << "\"";
    }
  }
}

namespace {
boost::container::flat_set<int> getOpenFds() {
  auto pid = getpid();
  auto dirname = to<std::string>("/proc/", pid, "/fd");

  boost::container::flat_set<int> fds;
  for (fs::directory_iterator it(dirname);
       it != fs::directory_iterator();
       ++it) {
    int fd = to<int>(it->path().filename().native());
    fds.insert(fd);
  }
  return fds;
}

template<class Runnable>
void checkFdLeak(const Runnable& r) {
  // Get the currently open fds.  Check that they are the same both before and
  // after calling the specified function.  We read the open fds from /proc.
  // (If we wanted to work even on systems that don't have /proc, we could
  // perhaps create and immediately close a socket both before and after
  // running the function, and make sure we got the same fd number both times.)
  auto fdsBefore = getOpenFds();
  r();
  auto fdsAfter = getOpenFds();
  EXPECT_EQ(fdsAfter.size(), fdsBefore.size());
}
}

// Make sure Subprocess doesn't leak any file descriptors
TEST(SimpleSubprocessTest, FdLeakTest) {
  // Normal execution
  checkFdLeak([] {
    Subprocess proc("true");
    EXPECT_EQ(0, proc.wait().exitStatus());
  });
  // Normal execution with pipes
  checkFdLeak([] {
    Subprocess proc("echo foo; echo bar >&2",
                    Subprocess::pipeStdout() | Subprocess::pipeStderr());
    auto p = proc.communicate();
    EXPECT_EQ("foo\n", p.first);
    EXPECT_EQ("bar\n", p.second);
    proc.waitChecked();
  });

  // Test where the exec call fails()
  checkFdLeak([] {
    EXPECT_SPAWN_ERROR(ENOENT, "failed to execute", "/no/such/file");
  });
  // Test where the exec call fails() with pipes
  checkFdLeak([] {
    try {
      Subprocess proc(std::vector<std::string>({"/no/such/file"}),
                      Subprocess::pipeStdout().stderr(Subprocess::PIPE));
      ADD_FAILURE() << "expected an error when running /no/such/file";
    } catch (const SubprocessSpawnError& ex) {
      EXPECT_EQ(ENOENT, ex.errnoValue());
    }
  });
}

TEST(ParentDeathSubprocessTest, ParentDeathSignal) {
  // Find out where we are.
  static constexpr size_t pathLength = 2048;
  char buf[pathLength + 1];
  int r = readlink("/proc/self/exe", buf, pathLength);
  CHECK_ERR(r);
  buf[r] = '\0';

  fs::path helper(buf);
  helper.remove_filename();
  helper /= "subprocess_test_parent_death_helper";

  fs::path tempFile(fs::temp_directory_path() / fs::unique_path());

  std::vector<std::string> args {helper.string(), tempFile.string()};
  Subprocess proc(args);
  // The helper gets killed by its child, see details in
  // SubprocessTestParentDeathHelper.cpp
  ASSERT_EQ(SIGKILL, proc.wait().killSignal());

  // Now wait for the file to be created, see details in
  // SubprocessTestParentDeathHelper.cpp
  while (!fs::exists(tempFile)) {
    usleep(20000);  // 20ms
  }

  fs::remove(tempFile);
}

TEST(PopenSubprocessTest, PopenRead) {
  Subprocess proc("ls /", Subprocess::pipeStdout());
  int found = 0;
  gen::byLine(File(proc.stdout())) |
    [&] (StringPiece line) {
      if (line == "etc" || line == "bin" || line == "usr") {
        ++found;
      }
    };
  EXPECT_EQ(3, found);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, SimpleRead) {
  Subprocess proc(std::vector<std::string>{ "/bin/echo", "-n", "foo", "bar"},
                  Subprocess::pipeStdout());
  auto p = proc.communicate();
  EXPECT_EQ("foo bar", p.first);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, BigWrite) {
  const int numLines = 1 << 20;
  std::string line("hello\n");
  std::string data;
  data.reserve(numLines * line.size());
  for (int i = 0; i < numLines; ++i) {
    data.append(line);
  }

  Subprocess proc("wc -l", Subprocess::pipeStdin() | Subprocess::pipeStdout());
  auto p = proc.communicate(data);
  EXPECT_EQ(folly::format("{}\n", numLines).str(), p.first);
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, Duplex) {
  // Take 10MB of data and pass them through a filter.
  // One line, as tr is line-buffered
  const int bytes = 10 << 20;
  std::string line(bytes, 'x');

  Subprocess proc("tr a-z A-Z",
                  Subprocess::pipeStdin() | Subprocess::pipeStdout());
  auto p = proc.communicate(line);
  EXPECT_EQ(bytes, p.first.size());
  EXPECT_EQ(std::string::npos, p.first.find_first_not_of('X'));
  proc.waitChecked();
}

TEST(CommunicateSubprocessTest, Duplex2) {
  checkFdLeak([] {
    // Pipe 200,000 lines through sed
    const size_t numCopies = 100000;
    auto iobuf = IOBuf::copyBuffer("this is a test\nanother line\n");
    IOBufQueue input;
    for (int n = 0; n < numCopies; ++n) {
      input.append(iobuf->clone());
    }

    std::vector<std::string> cmd({
      "sed", "-u",
      "-e", "s/a test/a successful test/",
      "-e", "/^another line/w/dev/stderr",
    });
    auto options = Subprocess::pipeStdin().pipeStdout().pipeStderr().usePath();
    Subprocess proc(cmd, options);
    auto out = proc.communicateIOBuf(std::move(input));
    proc.waitChecked();

    // Convert stdout and stderr to strings so we can call split() on them.
    fbstring stdoutStr;
    if (out.first.front()) {
      stdoutStr = out.first.move()->moveToFbString();
    }
    fbstring stderrStr;
    if (out.second.front()) {
      stderrStr = out.second.move()->moveToFbString();
    }

    // stdout should be a copy of stdin, with "a test" replaced by
    // "a successful test"
    std::vector<StringPiece> stdoutLines;
    split('\n', stdoutStr, stdoutLines);
    EXPECT_EQ(numCopies * 2 + 1, stdoutLines.size());
    // Strip off the trailing empty line
    if (!stdoutLines.empty()) {
      EXPECT_EQ("", stdoutLines.back());
      stdoutLines.pop_back();
    }
    size_t linenum = 0;
    for (const auto& line : stdoutLines) {
      if ((linenum & 1) == 0) {
        EXPECT_EQ("this is a successful test", line);
      } else {
        EXPECT_EQ("another line", line);
      }
      ++linenum;
    }

    // stderr should only contain the lines containing "another line"
    std::vector<StringPiece> stderrLines;
    split('\n', stderrStr, stderrLines);
    EXPECT_EQ(numCopies + 1, stderrLines.size());
    // Strip off the trailing empty line
    if (!stderrLines.empty()) {
      EXPECT_EQ("", stderrLines.back());
      stderrLines.pop_back();
    }
    for (const auto& line : stderrLines) {
      EXPECT_EQ("another line", line);
    }
  });
}

namespace {

bool readToString(int fd, std::string& buf, size_t maxSize) {
  size_t bytesRead = 0;

  buf.resize(maxSize);
  char* dest = &buf.front();
  size_t remaining = maxSize;

  ssize_t n = -1;
  while (remaining) {
    n = ::read(fd, dest, remaining);
    if (n == -1) {
      if (errno == EINTR) {
        continue;
      }
      if (errno == EAGAIN) {
        break;
      }
      PCHECK("read failed");
    } else if (n == 0) {
      break;
    }
    dest += n;
    remaining -= n;
  }

  buf.resize(dest - buf.data());
  return (n == 0);
}

}  // namespace

TEST(CommunicateSubprocessTest, Chatty) {
  checkFdLeak([] {
    const int lineCount = 1000;

    int wcount = 0;
    int rcount = 0;

    auto options = Subprocess::pipeStdin().pipeStdout().pipeStderr().usePath();
    std::vector<std::string> cmd {
      "sed",
      "-u",
      "-e",
      "s/a test/a successful test/",
    };

    Subprocess proc(cmd, options);

    auto writeCallback = [&] (int pfd, int cfd) -> bool {
      EXPECT_EQ(0, cfd);  // child stdin
      EXPECT_EQ(rcount, wcount);  // chatty, one read for every write

      auto msg = folly::to<std::string>("a test ", wcount, "\n");

      // Not entirely kosher, we should handle partial writes, but this is
      // fine for writes <= PIPE_BUF
      EXPECT_EQ(msg.size(), writeFull(pfd, msg.data(), msg.size()));

      ++wcount;
      proc.enableNotifications(0, false);

      return (wcount == lineCount);
    };

    auto readCallback = [&] (int pfd, int cfd) -> bool {
      EXPECT_EQ(1, cfd);  // child stdout
      EXPECT_EQ(wcount, rcount + 1);

      auto expected =
        folly::to<std::string>("a successful test ", rcount, "\n");

      std::string lineBuf;

      // Not entirely kosher, we should handle partial reads, but this is
      // fine for reads <= PIPE_BUF
      bool r = readToString(pfd, lineBuf, expected.size() + 1);

      EXPECT_TRUE(!r || (rcount + 1 == lineCount)); // may read EOF at end
      EXPECT_EQ(expected, lineBuf);

      ++rcount;
      if (rcount != lineCount) {
        proc.enableNotifications(0, true);
      }

      return (rcount == lineCount);
    };

    proc.communicate(readCallback, writeCallback);

    EXPECT_EQ(lineCount, wcount);
    EXPECT_EQ(lineCount, rcount);

    EXPECT_EQ(0, proc.wait().exitStatus());
  });
}
<|endoftext|>