<commit_before>// Copyright (c) 2012 Plenluno All rights reserved.

#include <assert.h>
#include <stdio.h>
#include <stdarg.h>
#include <string>

#include "libj/collection.h"
#include "libj/console.h"
#include "libj/json.h"
#include "libj/map.h"
#include "libj/string.h"

#pragma GCC diagnostic ignored "-Wformat-security"

namespace libj {
namespace console {

static Level logLevel = DEBUG;

static Boolean isPrintable(Level level) {
    return level != OFF && logLevel <= level;
}

void setLevel(Level level) {
    logLevel = level;
}

static Color foreDebug   = DEFAULT;
static Color backDebug   = DEFAULT;
static Color foreInfo    = DEFAULT;
static Color backInfo    = DEFAULT;
static Color foreNormal  = DEFAULT;
static Color backNormal  = DEFAULT;
static Color foreWarning = DEFAULT;
static Color backWarning = DEFAULT;
static Color foreError   = DEFAULT;
static Color backError   = DEFAULT;

static const char* fore(Color color) {
    switch (color) {
    case DEFAULT:
        return "";
    case BLACK:
        return "\033[30m";
    case RED:
        return "\033[31m";
    case GREEN:
        return "\033[32m";
    case YELLOW:
        return "\033[33m";
    case BLUE:
        return "\033[34m";
    case MAGENTA:
        return "\033[35m";
    case CYAN:
        return "\033[36m";
    case WHITE:
        return "\033[37m";
    }
}

static const char* back(Color color) {
    switch (color) {
    case DEFAULT:
        return "";
    case BLACK:
        return "\033[40m";
    case RED:
        return "\033[41m";
    case GREEN:
        return "\033[42m";
    case YELLOW:
        return "\033[43m";
    case BLUE:
        return "\033[44m";
    case MAGENTA:
        return "\033[45m";
    case CYAN:
        return "\033[46m";
    case WHITE:
        return "\033[47m";
    }
}

static const char* fore(Level level) {
    Color color;
    switch (level) {
    case DEBUG:
        color = foreDebug;
        break;
    case INFO:
        color = foreInfo;
        break;
    case NORMAL:
        color = foreNormal;
        break;
    case WARNING:
        color = foreWarning;
        break;
    case ERROR:
        color = foreError;
        break;
    case OFF:
        color = DEFAULT;
        break;
    }
    return fore(color);
}

static const char* back(Level level) {
    Color color;
    switch (level) {
    case DEBUG:
        color = backDebug;
        break;
    case INFO:
        color = backInfo;
        break;
    case NORMAL:
        color = backNormal;
        break;
    case WARNING:
        color = backWarning;
        break;
    case ERROR:
        color = backError;
        break;
    case OFF:
        color = DEFAULT;
        break;
    }
    return back(color);
}

void setForegroundColor(Level level, Color color) {
    switch (level) {
    case DEBUG:
        foreDebug = color;
        break;
    case INFO:
        foreInfo = color;
        break;
    case NORMAL:
        foreNormal = color;
        break;
    case WARNING:
        foreWarning = color;
        break;
    case ERROR:
        foreError = color;
        break;
    case OFF:
        break;
    }
}

void setBackgroundColor(Level level, Color color) {
    switch (level) {
    case DEBUG:
        backDebug = color;
        break;
    case INFO:
        backInfo = color;
        break;
    case NORMAL:
        backNormal = color;
        break;
    case WARNING:
        backWarning = color;
        break;
    case ERROR:
        backError = color;
        break;
    case OFF:
        break;
    }
}

void printf(Level level, const char* fmt, ...) {
    if (!isPrintable(level)) return;

    FILE* out = 0;
    switch (level) {
    case DEBUG:
    case INFO:
    case NORMAL:
        out = stdout;
        break;
    case WARNING:
    case ERROR:
        out = stderr;
        break;
    case OFF:
        assert(false);
    }

    va_list argp;
    va_start(argp, fmt);
    fprintf(out, fore(level));
    fprintf(out, back(level));
    vfprintf(out, fmt, argp);
    fprintf(out, "\033[0m");
    fprintf(out, "\033[0m");
}

static String::CPtr toString(const Value& val) {
    String::CPtr s = String::null();
    if (val.instanceof(Type<Map>::id()) ||
        val.instanceof(Type<Collection>::id())) {
        s = json::stringify(val);
    } else {
        s = String::valueOf(val);
    }
    return s;
}

void printv(
    Level level, const char* fmt,
    const Value& v0, const Value& v1,
    const Value& v2, const Value& v3,
    const Value& v4, const Value& v5,
    const Value& v6, const Value& v7,
    const Value& v8, const Value& v9) {
    if (!isPrintable(level)) return;

    const Size kMax = 10;
    std::string format(fmt);
    Size count = 0;
    size_t pos = 0;
    while (1) {
        pos = format.find("%v", pos);
        if (pos == std::string::npos) {
            break;
        } else {
            format.replace(pos, 2, "%s");
            count++;
        }
    }

    String::CPtr s[kMax];
    s[0] = toString(v0);
    s[1] = toString(v1);
    s[2] = toString(v2);
    s[3] = toString(v3);
    s[4] = toString(v4);
    s[5] = toString(v5);
    s[6] = toString(v6);
    s[7] = toString(v7);
    s[8] = toString(v8);
    s[9] = toString(v9);
    for (Size i = count; i < kMax; i++) {
        format += "%s";
        s[i] = String::null();
    }

    printf(level, format.c_str(),
        s[0] ? s[0]->toStdString().c_str() : "",
        s[1] ? s[1]->toStdString().c_str() : "",
        s[2] ? s[2]->toStdString().c_str() : "",
        s[3] ? s[3]->toStdString().c_str() : "",
        s[4] ? s[4]->toStdString().c_str() : "",
        s[5] ? s[5]->toStdString().c_str() : "",
        s[6] ? s[6]->toStdString().c_str() : "",
        s[7] ? s[7]->toStdString().c_str() : "",
        s[8] ? s[8]->toStdString().c_str() : "",
        s[9] ? s[9]->toStdString().c_str() : "");
}

void log(const char* fmt, ...) {
    if (!isPrintable(NORMAL)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(NORMAL, fmt, argp);
    ::printf("\n");
}

void debug(const char* fmt, ...) {
    if (!isPrintable(DEBUG)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(DEBUG, fmt, argp);
    ::printf("\n");
}

void info(const char* fmt, ...) {
    if (!isPrintable(INFO)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(INFO, fmt, argp);
    ::printf("\n");
}

void warn(const char* fmt, ...) {
    if (!isPrintable(WARNING)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(WARNING, fmt, argp);
    ::printf("\n");
}

void error(const char* fmt, ...) {
    if (!isPrintable(ERROR)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(ERROR, fmt, argp);
    ::printf("\n");
}

Boolean log(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(NORMAL)) {
        log(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean debug(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(DEBUG)) {
        debug(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean info(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(INFO)) {
        info(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean warn(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(WARNING)) {
        warn(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean error(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(ERROR)) {
        error(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

}  // namespace console
}  // namespace libj
<commit_msg>delete g++ warnings<commit_after>// Copyright (c) 2012 Plenluno All rights reserved.

#include <assert.h>
#include <stdio.h>
#include <stdarg.h>
#include <string>

#include "libj/collection.h"
#include "libj/console.h"
#include "libj/json.h"
#include "libj/map.h"
#include "libj/string.h"

#pragma GCC diagnostic ignored "-Wformat-security"

namespace libj {
namespace console {

static Level logLevel = DEBUG;

static Boolean isPrintable(Level level) {
    return level != OFF && logLevel <= level;
}

void setLevel(Level level) {
    logLevel = level;
}

static Color foreDebug   = DEFAULT;
static Color backDebug   = DEFAULT;
static Color foreInfo    = DEFAULT;
static Color backInfo    = DEFAULT;
static Color foreNormal  = DEFAULT;
static Color backNormal  = DEFAULT;
static Color foreWarning = DEFAULT;
static Color backWarning = DEFAULT;
static Color foreError   = DEFAULT;
static Color backError   = DEFAULT;

static const char* fore(Color color) {
    switch (color) {
    case DEFAULT:
        return "";
    case BLACK:
        return "\033[30m";
    case RED:
        return "\033[31m";
    case GREEN:
        return "\033[32m";
    case YELLOW:
        return "\033[33m";
    case BLUE:
        return "\033[34m";
    case MAGENTA:
        return "\033[35m";
    case CYAN:
        return "\033[36m";
    case WHITE:
        return "\033[37m";
    default:
        assert(false);
        return "";
    }
}

static const char* back(Color color) {
    switch (color) {
    case DEFAULT:
        return "";
    case BLACK:
        return "\033[40m";
    case RED:
        return "\033[41m";
    case GREEN:
        return "\033[42m";
    case YELLOW:
        return "\033[43m";
    case BLUE:
        return "\033[44m";
    case MAGENTA:
        return "\033[45m";
    case CYAN:
        return "\033[46m";
    case WHITE:
        return "\033[47m";
    default:
        assert(false);
        return "";
    }
}

static const char* fore(Level level) {
    Color color;
    switch (level) {
    case DEBUG:
        color = foreDebug;
        break;
    case INFO:
        color = foreInfo;
        break;
    case NORMAL:
        color = foreNormal;
        break;
    case WARNING:
        color = foreWarning;
        break;
    case ERROR:
        color = foreError;
        break;
    case OFF:
        color = DEFAULT;
        break;
    }
    return fore(color);
}

static const char* back(Level level) {
    Color color;
    switch (level) {
    case DEBUG:
        color = backDebug;
        break;
    case INFO:
        color = backInfo;
        break;
    case NORMAL:
        color = backNormal;
        break;
    case WARNING:
        color = backWarning;
        break;
    case ERROR:
        color = backError;
        break;
    case OFF:
        color = DEFAULT;
        break;
    }
    return back(color);
}

void setForegroundColor(Level level, Color color) {
    switch (level) {
    case DEBUG:
        foreDebug = color;
        break;
    case INFO:
        foreInfo = color;
        break;
    case NORMAL:
        foreNormal = color;
        break;
    case WARNING:
        foreWarning = color;
        break;
    case ERROR:
        foreError = color;
        break;
    case OFF:
        break;
    }
}

void setBackgroundColor(Level level, Color color) {
    switch (level) {
    case DEBUG:
        backDebug = color;
        break;
    case INFO:
        backInfo = color;
        break;
    case NORMAL:
        backNormal = color;
        break;
    case WARNING:
        backWarning = color;
        break;
    case ERROR:
        backError = color;
        break;
    case OFF:
        break;
    }
}

void printf(Level level, const char* fmt, ...) {
    if (!isPrintable(level)) return;

    FILE* out = 0;
    switch (level) {
    case DEBUG:
    case INFO:
    case NORMAL:
        out = stdout;
        break;
    case WARNING:
    case ERROR:
        out = stderr;
        break;
    case OFF:
        assert(false);
    }

    va_list argp;
    va_start(argp, fmt);
    fprintf(out, fore(level));
    fprintf(out, back(level));
    vfprintf(out, fmt, argp);
    fprintf(out, "\033[0m");
    fprintf(out, "\033[0m");
}

static String::CPtr toString(const Value& val) {
    String::CPtr s = String::null();
    if (val.instanceof(Type<Map>::id()) ||
        val.instanceof(Type<Collection>::id())) {
        s = json::stringify(val);
    } else {
        s = String::valueOf(val);
    }
    return s;
}

void printv(
    Level level, const char* fmt,
    const Value& v0, const Value& v1,
    const Value& v2, const Value& v3,
    const Value& v4, const Value& v5,
    const Value& v6, const Value& v7,
    const Value& v8, const Value& v9) {
    if (!isPrintable(level)) return;

    const Size kMax = 10;
    std::string format(fmt);
    Size count = 0;
    size_t pos = 0;
    while (1) {
        pos = format.find("%v", pos);
        if (pos == std::string::npos) {
            break;
        } else {
            format.replace(pos, 2, "%s");
            count++;
        }
    }

    String::CPtr s[kMax];
    s[0] = toString(v0);
    s[1] = toString(v1);
    s[2] = toString(v2);
    s[3] = toString(v3);
    s[4] = toString(v4);
    s[5] = toString(v5);
    s[6] = toString(v6);
    s[7] = toString(v7);
    s[8] = toString(v8);
    s[9] = toString(v9);
    for (Size i = count; i < kMax; i++) {
        format += "%s";
        s[i] = String::null();
    }

    printf(level, format.c_str(),
        s[0] ? s[0]->toStdString().c_str() : "",
        s[1] ? s[1]->toStdString().c_str() : "",
        s[2] ? s[2]->toStdString().c_str() : "",
        s[3] ? s[3]->toStdString().c_str() : "",
        s[4] ? s[4]->toStdString().c_str() : "",
        s[5] ? s[5]->toStdString().c_str() : "",
        s[6] ? s[6]->toStdString().c_str() : "",
        s[7] ? s[7]->toStdString().c_str() : "",
        s[8] ? s[8]->toStdString().c_str() : "",
        s[9] ? s[9]->toStdString().c_str() : "");
}

void log(const char* fmt, ...) {
    if (!isPrintable(NORMAL)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(NORMAL, fmt, argp);
    ::printf("\n");
}

void debug(const char* fmt, ...) {
    if (!isPrintable(DEBUG)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(DEBUG, fmt, argp);
    ::printf("\n");
}

void info(const char* fmt, ...) {
    if (!isPrintable(INFO)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(INFO, fmt, argp);
    ::printf("\n");
}

void warn(const char* fmt, ...) {
    if (!isPrintable(WARNING)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(WARNING, fmt, argp);
    ::printf("\n");
}

void error(const char* fmt, ...) {
    if (!isPrintable(ERROR)) return;

    va_list argp;
    va_start(argp, fmt);
    printf(ERROR, fmt, argp);
    ::printf("\n");
}

Boolean log(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(NORMAL)) {
        log(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean debug(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(DEBUG)) {
        debug(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean info(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(INFO)) {
        info(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean warn(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(WARNING)) {
        warn(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

Boolean error(const Value& val) {
    String::CPtr s = toString(val);
    if (s && isPrintable(ERROR)) {
        error(s->toStdString().c_str());
        return true;
    } else {
        return false;
    }
}

}  // namespace console
}  // namespace libj
<|endoftext|>


<commit_before>#include "control.hpp"
#include "math.hpp"
#include "model.hpp"

#define GLFW_INCLUDE_NONE
#include <GLFW/glfw3.h>
#include <GL/glu.h>
#include <SOIL/SOIL.h>

#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>

#include "omp.h"

namespace Control {

	void control::deadzone(float &x, float &y) {
		float r = sqrt(x*x + y*y), theta = atan2(y, x);
		if(r < r_dead) {
			x = y = 0;
		} else {
			x = (x/r - r_dead) / (1-r_dead);
			y = (y/r - r_dead) / (1-r_dead);
		}
	}

	bool control::init(std::atomic_bool &alive) {
		//using namespace Util;
		// Task 1: view before model (splash)
		using namespace View;
		glfwSetInputMode(viewer.win, GLFW_STICKY_KEYS, 1);
		glfwMakeContextCurrent(viewer.win);

		using namespace Model;
		mesh_t mesh(150, 150,
		[](float s, float t, std::vector<float> &vertices) {
			using namespace Math;
			auto theta = s*M_PI*2, phi = t*M_PI;
			vertices.emplace_back(cos(theta)*sin(phi)); // X
			vertices.emplace_back(sin(theta)*sin(phi)); // Y
			vertices.emplace_back(cos(phi));            // Z
		});

		glGenBuffers(1, &viewer.ids[view::e_id_vbuf]);
		glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
		glBufferData(GL_ARRAY_BUFFER, mesh.vertices.size() * sizeof(float),
			(void*) &mesh.vertices[0], GL_STATIC_DRAW);

		glGenBuffers(1, &viewer.ids[view::e_id_fbuf]);
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
			viewer.ids[view::e_id_fbuf]);
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh.faces.size() * sizeof(int),
			(void*) &mesh.faces[0], GL_STATIC_DRAW);
		viewer.nTriangles = mesh.faces.size()/3;
		return alive;

		// Task 2: wavefront obj model loading
		/*using namespace Model;
		obj_t object;
		auto status = obj_t::load(this -> mpath, object);
		if(status != obj_t::e_ok) {
			errors.push_back("The model failed to load.");
			return alive = false;
		}

		glGenBuffers(view::e_id_model-view::e_id_vbuf,
				&viewer.ids[view::e_id_vbuf]);
		// TODO Use more than first range of each obj type
		if(object.v_beg.size() == 0) {
			errors.push_back("The loaded model does not contain vertices.");
			return alive = false;
		} else {
			auto v0 = object.v_beg[0], v1 = object.v_end[0];
			glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
			glBufferData(GL_ARRAY_BUFFER, v1-v0,
					(void*)(&object.floats[v0]), GL_STATIC_DRAW);
		}
		if(object.vn_beg.size() != 0) {
			auto vn0 = object.vn_beg[0], vn1 = object.vn_end[0];
			glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vnbuf]);
			glBufferData(GL_ARRAY_BUFFER, (vn1-vn0)*sizeof(float),
					(void*)(&object.floats[vn0]), GL_STATIC_DRAW);
		}
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
				viewer.ids[view::e_id_fbuf]);
		int f0, f1;
		if(object.f0_beg.size() != 0) {
			f0 = object.f0_beg[0];
			f1 = object.f0_end[0];
		} else if(object.f1_beg.size() != 0) {
			f0 = object.f1_beg[0];
			f1 = object.f1_end[0];
		} else if(object.f2_beg.size() != 0) {
			f0 = object.f2_beg[0];
			f1 = object.f2_end[0];
		} else if(object.f3_beg.size() != 0) {
			f0 = object.f3_beg[0];
			f1 = object.f3_end[0];
		} else {
			errors.push_back("The loaded model does not contain faces.");
			return alive = false;
		}
		viewer.nTriangles = (f1-f0)/3;
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, (f1-f0)*sizeof(int),
				(void*)(&object.ints[f0]), GL_STATIC_DRAW);
		return alive;*/

		/*
		// Task 2: model loading
		using namespace Model::Ply;
 		Header model(this -> mpath);
 		if(model.status) {
			std::cout << model.statusContext << std::endl;
			return;
		}
		auto start = begin(model.elements),
				 stop = end(model.elements);
		auto vertices = std::find_if(start, stop,
			[](Element const& el) -> bool {
				// TODO Change behavior based on the number of elements
				return el.name == "vertex" && !el.has_list;
		}), indices = std::find_if(start, stop,
			[](Element const& el) -> bool {
				int sz = el.properties.size();
				return el.name == "face"
					&& (el.has_list ? sz==1 : sz==3);
		});
		if(vertices == stop || indices == stop) {
			std::cout << "The model is valid, but does not match "
				"the anticipated structure." << std::endl;
			return;
		Task 3: view after model
		glGenBuffers(1, &viewer.ids[view::e_id_vbuf]);
		glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
		glBufferData(GL_ARRAY_BUFFER, vertices->data.size(),
				(void*)(&vertices->data[0]), GL_STATIC_DRAW);

		glGenBuffers(1, &viewer.ids[view::e_id_ibuf]);
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
				viewer.ids[view::e_id_ibuf]);
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices->data.size(),
				(void*)(&indices->data[0]), GL_STATIC_DRAW);
		viewer.nTriangles = indices -> instances;
		glUseProgram(viewer.ids[view::e_id_prog]);*/
	}

	bool control::poll(std::atomic_bool &alive) {
		std::ostringstream oss;
		if(alive)
			viewer.poll(alive);
		if(alive) {
			if(glfwGetKey(viewer.win, GLFW_KEY_ESCAPE) == GLFW_PRESS)
				alive = false;
			int nButtons;
			const uint8_t *buttons =
				glfwGetJoystickButtons(GLFW_JOYSTICK_1, &nButtons);
			for(int i = 0; i < nButtons; i++) {
				if(buttons[i] == GLFW_PRESS) {
					if(i == 8) alive = false;
					else if(i == 7)
						viewer.x = viewer.y = viewer.z = 0;
					else if(i == 4)
						viewer.y -= .1;
					else if(i == 5)
						viewer.y += .1;
				}
			}
			int nAxes;
			const GLfloat *axes =
				glfwGetJoystickAxes(GLFW_JOYSTICK_1, &nAxes);
			if(nAxes >= 2) {
				float x = axes[0], y = axes[1];
				deadzone(x, y);
				viewer.x -= x/20;
				viewer.z -= y/20;
			}
			if(nAxes >= 5) {
				float theta = axes[3], phi = axes[4];
				viewer.theta = theta*M_PI;
				viewer.phi = phi*M_PI;
			}
		}
		return alive;
	}

	bool control::run(std::atomic_bool &alive) {
		using namespace Model::Ply;
		auto delay = std::chrono::milliseconds(150);

		int frame = 0, dFrames = 0, fps = 0;
		auto kill = [&alive] {alive = false;};
		double t1 = glfwGetTime(), t2;
		while(true) {
			if(poll(alive)) viewer.run(alive);
			t2 = glfwGetTime() - t1;
			frame++;
			dFrames++;
			if(t2 > 1.0) {
				std::cout << "FPS: " << int(dFrames/t2) << std::endl;
				t1 += t2;
				dFrames = 0;
			}
			if(!alive) break;
		}
		return alive;
	}

	control::control(std::atomic_bool &alive, const char *mpath):
		task(), mpath(mpath), viewer(alive) {}
}
<commit_msg>Troubleshooting obj (unsuccessful)<commit_after>#include "control.hpp"
#include "math.hpp"
#include "model.hpp"

#define GLFW_INCLUDE_NONE
#include <GLFW/glfw3.h>
#include <GL/glu.h>
#include <SOIL/SOIL.h>

#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>

#include "omp.h"

namespace Control {

	void control::deadzone(float &x, float &y) {
		float r = sqrt(x*x + y*y), theta = atan2(y, x);
		if(r < r_dead) {
			x = y = 0;
		} else {
			x = (x/r - r_dead) / (1-r_dead);
			y = (y/r - r_dead) / (1-r_dead);
		}
	}

	bool control::init(std::atomic_bool &alive) {
		//using namespace Util;
		// Task 1: view before model (splash)
		using namespace View;
		glfwSetInputMode(viewer.win, GLFW_STICKY_KEYS, 1);
		glfwMakeContextCurrent(viewer.win);

		/*using namespace Model;
		mesh_t mesh(150, 150,
		[](float s, float t, std::vector<float> &vertices) {
			using namespace Math;
			auto theta = s*M_PI*2, phi = t*M_PI;
			vertices.emplace_back(cos(theta)*sin(phi)); // X
			vertices.emplace_back(sin(theta)*sin(phi)); // Y
			vertices.emplace_back(cos(phi));            // Z
		});

		glGenBuffers(1, &viewer.ids[view::e_id_vbuf]);
		glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
		glBufferData(GL_ARRAY_BUFFER, mesh.vertices.size() * sizeof(float),
			(void*) &mesh.vertices[0], GL_STATIC_DRAW);

		glGenBuffers(1, &viewer.ids[view::e_id_fbuf]);
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
			viewer.ids[view::e_id_fbuf]);
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh.faces.size() * sizeof(int),
			(void*) &mesh.faces[0], GL_STATIC_DRAW);
		viewer.nTriangles = mesh.faces.size()/3;
		return alive;*/

		// Task 2: wavefront obj model loading
		using namespace Model;
		obj_t object;
		auto status = obj_t::load(this -> mpath, object);
		if(status != obj_t::e_ok) {
			errors.push_back("The model failed to load.");
			return alive = false;
		}

		// TODO Use more than first range of each obj type
		if(object.v_beg.size() == 0) {
			errors.push_back("The loaded model does not contain vertices.");
			return alive = false;
		} else {
			auto v0 = object.v_beg[0], v1 = object.v_end[0];
			glGenBuffers(1, &viewer.ids[view::e_id_vbuf]);
			glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
			glBufferData(GL_ARRAY_BUFFER, v1-v0,
					(void*)(&object.floats[v0]), GL_STATIC_DRAW);
		}
		if(object.vn_beg.size() != 0) {
			auto vn0 = object.vn_beg[0], vn1 = object.vn_end[0];
			glGenBuffers(1, &viewer.ids[view::e_id_vnbuf]);
			glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vnbuf]);
			glBufferData(GL_ARRAY_BUFFER, (vn1-vn0)*sizeof(float),
					(void*)(&object.floats[vn0]), GL_STATIC_DRAW);
		}
		glGenBuffers(1, &viewer.ids[view::e_id_fbuf]);
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
				viewer.ids[view::e_id_fbuf]);
		int f0, f1;
		if(object.f0_beg.size() != 0) {
			f0 = object.f0_beg[0];
			f1 = object.f0_end[0];
		} else if(object.f1_beg.size() != 0) {
			f0 = object.f1_beg[0];
			f1 = object.f1_end[0];
		} else if(object.f2_beg.size() != 0) {
			f0 = object.f2_beg[0];
			f1 = object.f2_end[0];
		} else if(object.f3_beg.size() != 0) {
			f0 = object.f3_beg[0];
			f1 = object.f3_end[0];
		} else {
			errors.push_back("The loaded model does not contain faces.");
			return alive = false;
		}
		viewer.nTriangles = (f1-f0)/3;
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, (f1-f0)/3*sizeof(int),
				(void*)(&object.ints[f0]), GL_STATIC_DRAW);
		return alive;

		/*
		// Task 2: model loading
		using namespace Model::Ply;
 		Header model(this -> mpath);
 		if(model.status) {
			std::cout << model.statusContext << std::endl;
			return;
		}
		auto start = begin(model.elements),
				 stop = end(model.elements);
		auto vertices = std::find_if(start, stop,
			[](Element const& el) -> bool {
				// TODO Change behavior based on the number of elements
				return el.name == "vertex" && !el.has_list;
		}), indices = std::find_if(start, stop,
			[](Element const& el) -> bool {
				int sz = el.properties.size();
				return el.name == "face"
					&& (el.has_list ? sz==1 : sz==3);
		});
		if(vertices == stop || indices == stop) {
			std::cout << "The model is valid, but does not match "
				"the anticipated structure." << std::endl;
			return;
		Task 3: view after model
		glGenBuffers(1, &viewer.ids[view::e_id_vbuf]);
		glBindBuffer(GL_ARRAY_BUFFER, viewer.ids[view::e_id_vbuf]);
		glBufferData(GL_ARRAY_BUFFER, vertices->data.size(),
				(void*)(&vertices->data[0]), GL_STATIC_DRAW);

		glGenBuffers(1, &viewer.ids[view::e_id_ibuf]);
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,
				viewer.ids[view::e_id_ibuf]);
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices->data.size(),
				(void*)(&indices->data[0]), GL_STATIC_DRAW);
		viewer.nTriangles = indices -> instances;
		glUseProgram(viewer.ids[view::e_id_prog]);*/
	}

	bool control::poll(std::atomic_bool &alive) {
		std::ostringstream oss;
		if(alive)
			viewer.poll(alive);
		if(alive) {
			if(glfwGetKey(viewer.win, GLFW_KEY_ESCAPE) == GLFW_PRESS)
				alive = false;
			int nButtons;
			const uint8_t *buttons =
				glfwGetJoystickButtons(GLFW_JOYSTICK_1, &nButtons);
			for(int i = 0; i < nButtons; i++) {
				if(buttons[i] == GLFW_PRESS) {
					if(i == 8) alive = false;
					else if(i == 7)
						viewer.x = viewer.y = viewer.z = 0;
					else if(i == 4)
						viewer.y -= .1;
					else if(i == 5)
						viewer.y += .1;
				}
			}
			int nAxes;
			const GLfloat *axes =
				glfwGetJoystickAxes(GLFW_JOYSTICK_1, &nAxes);
			if(nAxes >= 2) {
				float x = axes[0], y = axes[1];
				deadzone(x, y);
				viewer.x -= x/20;
				viewer.z -= y/20;
			}
			if(nAxes >= 5) {
				float theta = axes[3], phi = axes[4];
				viewer.theta = theta*M_PI;
				viewer.phi = phi*M_PI;
			}
		}
		return alive;
	}

	bool control::run(std::atomic_bool &alive) {
		using namespace Model::Ply;
		auto delay = std::chrono::milliseconds(150);

		int frame = 0, dFrames = 0, fps = 0;
		auto kill = [&alive] {alive = false;};
		double t1 = glfwGetTime(), t2;
		while(true) {
			if(poll(alive)) viewer.run(alive);
			t2 = glfwGetTime() - t1;
			frame++;
			dFrames++;
			if(t2 > 1.0) {
				std::cout << "FPS: " << int(dFrames/t2) << std::endl;
				t1 += t2;
				dFrames = 0;
			}
			if(!alive) break;
		}
		return alive;
	}

	control::control(std::atomic_bool &alive, const char *mpath):
		task(), mpath(mpath), viewer(alive) {}
}
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>
#include <string>
#include <cmath>

#include <lua.hpp>
#include <luaT.h>
#include <thpp/Tensor.h>
#include <thpp/Storage.h>
#include <fblualib/LuaUtils.h>

#include "utils.hpp"

namespace {

const int blankLabel = 0;

template <class T>
int forwardBackward(lua_State* L) {
    const thpp::Tensor<T> input     = fblualib::luaGetTensorChecked<T>(L, 1);
    const thpp::Tensor<int> targets = fblualib::luaGetTensorChecked<int>(L, 2);
    const bool forwardOnly          = lua_toboolean(L, 3);
    thpp::Tensor<T> gradInput       = fblualib::luaGetTensorChecked<T>(L, 4);

    const int nFrame      = input.size(0);
    const int inputLength = input.size(1);
    const int nClasses    = input.size(2);
    const int maxTargetlength = targets.size(1);
    if (!forwardOnly) {
        gradInput.resizeAs(input);
        gradInput.fill(LogMath<T>::logZero);
    }

    thpp::Tensor<T> losses({nFrame});
    losses.fill(0);

    #pragma omp parallel for
    for (int i = 0; i < nFrame; ++i) {
        const thpp::Tensor<T>& input_i = input[i];
        const int* targetData_i = targets[i].data();
        const int targetLength = zeroPadArrayLength(targetData_i, maxTargetlength);
        const int nSegment = 2 * targetLength + 1;

        // compute forward variables
        thpp::Tensor<T> fvars({inputLength, nSegment});
        fvars.fill(LogMath<T>::logZero);
        fvars.at({0, 0}) = input_i.at({0, 0});
        if (nSegment > 1) {
            fvars.at({0, 1}) = input_i.at({0, targetData_i[0]});
        }
        for (int t = 1; t < inputLength; ++t) {
            const thpp::Tensor<T>& currLogActs = input_i[t];
            const thpp::Tensor<T>& prefFvars = fvars[t-1];
            int sBegin = std::max(0, nSegment - (2 * inputLength - t));
            int sEnd = std::min(nSegment, 2 * (t + 1));
            for (int s = sBegin; s < sEnd; ++s) { // FIXME: < or <= ??
                T fv;
                if (s % 2 == 1) { // non-blank
                    int labelIndex = s/2;
                    int label = targetData_i[labelIndex];
                    fv = LogMath<T>::logAdd(prefFvars.at(s), prefFvars.at(s-1));
                    if (s > 1 && label != targetData_i[labelIndex-1]) {
                        fv = LogMath<T>::logAdd(fv, prefFvars.at(s-2));
                    }
                    fv = LogMath<T>::logMul(fv, currLogActs.at(label));
                } else { // blank
                    fv = prefFvars.at(s);
                    if (s > 0) {
                        fv = LogMath<T>::logAdd(fv, prefFvars.at(s-1));
                    }
                    fv = LogMath<T>::logMul(fv, currLogActs.at(0)); // 0 for blank
                }
                fvars.at({t,s}) = fv;
            }
        }

        // compute log-likelihood
        T logProb = fvars.at({inputLength-1, nSegment-1});
        if (nSegment > 1) {
            logProb = LogMath<T>::logAdd(logProb, fvars.at({inputLength-1, nSegment-2}));
        }
        losses.at(i) = (-logProb);

        if (!forwardOnly) {
            // compute backward variables
            thpp::Tensor<T> bvars({inputLength, nSegment});
            bvars.fill(LogMath<T>::logZero);
            bvars.at({inputLength-1, nSegment-1}) = LogMath<T>::logOne;
            if (nSegment > 1) {
                bvars.at({inputLength-1, nSegment-2}) = LogMath<T>::logOne;
            }
            for (int t = inputLength-2; t >= 0; --t) {
                const thpp::Tensor<T>& prevLogActs = input_i[t+1];
                const thpp::Tensor<T>& prevBvars = bvars[t+1];
                int sBegin = std::max(0, nSegment - (2 * inputLength - t));
                int sEnd = std::min(nSegment, 2 * (t + 1));
                for (int s = sBegin; s < sEnd; ++s) {
                    T bv;
                    if (s % 2 == 1) {
                        const int labelIndex = s/2;
                        int label = targetData_i[labelIndex];
                        bv = LogMath<T>::logAdd(
                            LogMath<T>::logMul(prevBvars.at(s), prevLogActs.at(label)),
                            LogMath<T>::logMul(prevBvars.at(s+1), prevLogActs.at(blankLabel)));
                        if (s < nSegment-2) {
                            const int prevLabel = targetData_i[labelIndex+1];
                            if (label != prevLabel) {
                                bv = LogMath<T>::logAdd(bv,
                                    LogMath<T>::logMul(prevBvars.at(s+2), prevLogActs.at(prevLabel)));
                            }
                        }
                    } else {
                        int labelIndex = s/2;
                        int label = targetData_i[labelIndex];
                        bv = LogMath<T>::logMul(prevBvars.at(s), prevLogActs.at(blankLabel));
                        if (s < nSegment-1) {
                            bv = LogMath<T>::logAdd(bv,
                                LogMath<T>::logMul(prevBvars.at(s+1), prevLogActs.at(label)));
                        }
                    }
                    bvars.at({t,s}) = bv;
                }
            }

            // compute gradients on inputs
            for (int t = 0; t < inputLength; ++t) {
                const thpp::Tensor<T>& currLogFv = fvars[t];
                const thpp::Tensor<T>& currLogBv = bvars[t];
                std::vector<T> logDeDy(nClasses, LogMath<T>::logZero);
                for (int s = 0; s < nSegment; ++s) {
                    int k = (s%2==1) ? targetData_i[s/2] : blankLabel;
                    logDeDy[k] = LogMath<T>::logAdd(logDeDy[k],
                        LogMath<T>::logMul(currLogFv.at(s), currLogBv.at(s)));
                }
                for (int k = 0; k < nClasses; ++k) {
                    gradInput.at({i,t,k}) = -LogMath<T>::safeExp(
                        LogMath<T>::logDiv(logDeDy[k], logProb));
                }
            }
        } // if (!forwardOnly)
    }

    // return loss
    fblualib::luaPushTensor(L, losses);
    return 1;
}

template <class T>
int naiveDecoding(lua_State* L) {
    const thpp::Tensor<T>& input = fblualib::luaGetTensorChecked<T>(L, 1);
    const int nFrame = input.size(0);
    const int inputLength = input.size(1);

    thpp::Tensor<long> rawPred_ = input.max(2).second; // [nFrame x inputLength]
    thpp::Tensor<int> rawPred({nFrame, inputLength});
    rawPred.copy(rawPred_);

    thpp::Tensor<int> pred({nFrame, inputLength});
    pred.fill(0);

    for (int i = 0; i < nFrame; ++i) {
        std::vector<int> predVec;
        const int* rawPredData_i = rawPred[i].data();
        int* predData_i = pred[i].data();
        for (int t = 0; t < inputLength; ++t) {
            if (rawPredData_i[t] != 0 && !(t > 0 && rawPredData_i[t] == rawPredData_i[t-1])) {
                predVec.push_back(rawPredData_i[t]);
            }
        }
        for (int j = 0; j < (int)predVec.size(); ++j) {
            predData_i[j] = predVec[j];
        }
    }

    fblualib::luaPushTensor(L, pred);
    fblualib::luaPushTensor(L, rawPred);
    return 2;
}

// register functions
template <class T>
class Registerer {
private:
    static const luaL_Reg functions_[];
public:
    static void registerFunctions(lua_State* L);
};

template <class T>
const luaL_Reg Registerer<T>::functions_[] = {
    {"CTC_forwardBackward", forwardBackward<T>},
    {"CTC_naiveDecoding", naiveDecoding<T>},
    {nullptr, nullptr},
};

template <class T>
void Registerer<T>::registerFunctions(lua_State* L) {
    luaT_pushmetatable(L, thpp::Tensor<T>::kLuaTypeName);
    luaT_registeratname(L, functions_, "nn");
    lua_pop(L, 1);
}

} // namespace

void initCtc(lua_State* L) {
    Registerer<float>::registerFunctions(L);
    Registerer<double>::registerFunctions(L);
}
<commit_msg>Bug fix<commit_after>#include <iostream>
#include <vector>
#include <string>
#include <cmath>

#include <lua.hpp>
#include <luaT.h>
#include <thpp/Tensor.h>
#include <thpp/Storage.h>
#include <fblualib/LuaUtils.h>

#include "utils.hpp"

namespace {

const int blankLabel = 0;

template <class T>
int forwardBackward(lua_State* L) {
    const thpp::Tensor<T> input     = fblualib::luaGetTensorChecked<T>(L, 1);
    const thpp::Tensor<int> targets = fblualib::luaGetTensorChecked<int>(L, 2);
    const bool forwardOnly          = lua_toboolean(L, 3);
    thpp::Tensor<T> gradInput       = fblualib::luaGetTensorChecked<T>(L, 4);

    const int nFrame      = input.size(0);
    const int inputLength = input.size(1);
    const int nClasses    = input.size(2);
    const int maxTargetlength = targets.size(1);
    if (!forwardOnly) {
        gradInput.resizeAs(input);
        gradInput.fill(LogMath<T>::logZero);
    }

    thpp::Tensor<T> losses({nFrame});
    losses.fill(0);

    #pragma omp parallel for
    for (int i = 0; i < nFrame; ++i) {
        const thpp::Tensor<T>& input_i = input[i];
        const int* targetData_i = targets[i].data();
        const int targetLength = zeroPadArrayLength(targetData_i, maxTargetlength);
        const int nSegment = 2 * targetLength + 1;

        // compute forward variables
        thpp::Tensor<T> fvars({inputLength, nSegment});
        fvars.fill(LogMath<T>::logZero);
        fvars.at({0, 0}) = input_i.at({0, 0});
        if (nSegment > 1) {
            fvars.at({0, 1}) = input_i.at({0, targetData_i[0]});
        }
        for (int t = 1; t < inputLength; ++t) {
            const thpp::Tensor<T>& currLogActs = input_i[t];
            const thpp::Tensor<T>& prefFvars = fvars[t-1];
            int sBegin = std::max(0, nSegment - (2 * (inputLength - t)));
            int sEnd = std::min(nSegment, 2 * (t + 1));
            for (int s = sBegin; s < sEnd; ++s) { // FIXME: < or <= ??
                T fv;
                if (s % 2 == 1) { // non-blank
                    int labelIndex = s/2;
                    int label = targetData_i[labelIndex];
                    fv = LogMath<T>::logAdd(prefFvars.at(s), prefFvars.at(s-1));
                    if (s > 1 && label != targetData_i[labelIndex-1]) {
                        fv = LogMath<T>::logAdd(fv, prefFvars.at(s-2));
                    }
                    fv = LogMath<T>::logMul(fv, currLogActs.at(label));
                } else { // blank
                    fv = prefFvars.at(s);
                    if (s > 0) {
                        fv = LogMath<T>::logAdd(fv, prefFvars.at(s-1));
                    }
                    fv = LogMath<T>::logMul(fv, currLogActs.at(0)); // 0 for blank
                }
                fvars.at({t,s}) = fv;
            }
        }

        // compute log-likelihood
        T logProb = fvars.at({inputLength-1, nSegment-1});
        if (nSegment > 1) {
            logProb = LogMath<T>::logAdd(logProb, fvars.at({inputLength-1, nSegment-2}));
        }
        losses.at(i) = (-logProb);

        if (!forwardOnly) {
            // compute backward variables
            thpp::Tensor<T> bvars({inputLength, nSegment});
            bvars.fill(LogMath<T>::logZero);
            bvars.at({inputLength-1, nSegment-1}) = LogMath<T>::logOne;
            if (nSegment > 1) {
                bvars.at({inputLength-1, nSegment-2}) = LogMath<T>::logOne;
            }
            for (int t = inputLength-2; t >= 0; --t) {
                const thpp::Tensor<T>& prevLogActs = input_i[t+1];
                const thpp::Tensor<T>& prevBvars = bvars[t+1];
                int sBegin = std::max(0, nSegment - (2 * (inputLength - t)));
                int sEnd = std::min(nSegment, 2 * (t + 1));
                for (int s = sBegin; s < sEnd; ++s) {
                    T bv;
                    if (s % 2 == 1) {
                        const int labelIndex = s/2;
                        int label = targetData_i[labelIndex];
                        bv = LogMath<T>::logAdd(
                            LogMath<T>::logMul(prevBvars.at(s), prevLogActs.at(label)),
                            LogMath<T>::logMul(prevBvars.at(s+1), prevLogActs.at(blankLabel)));
                        if (s < nSegment-2) {
                            const int prevLabel = targetData_i[labelIndex+1];
                            if (label != prevLabel) {
                                bv = LogMath<T>::logAdd(bv,
                                    LogMath<T>::logMul(prevBvars.at(s+2), prevLogActs.at(prevLabel)));
                            }
                        }
                    } else {
                        int labelIndex = s/2;
                        int label = targetData_i[labelIndex];
                        bv = LogMath<T>::logMul(prevBvars.at(s), prevLogActs.at(blankLabel));
                        if (s < nSegment-1) {
                            bv = LogMath<T>::logAdd(bv,
                                LogMath<T>::logMul(prevBvars.at(s+1), prevLogActs.at(label)));
                        }
                    }
                    bvars.at({t,s}) = bv;
                }
            }

            // compute gradients on inputs
            for (int t = 0; t < inputLength; ++t) {
                const thpp::Tensor<T>& currLogFv = fvars[t];
                const thpp::Tensor<T>& currLogBv = bvars[t];
                std::vector<T> logDeDy(nClasses, LogMath<T>::logZero);
                for (int s = 0; s < nSegment; ++s) {
                    int k = (s%2==1) ? targetData_i[s/2] : blankLabel;
                    logDeDy[k] = LogMath<T>::logAdd(logDeDy[k],
                        LogMath<T>::logMul(currLogFv.at(s), currLogBv.at(s)));
                }
                for (int k = 0; k < nClasses; ++k) {
                    gradInput.at({i,t,k}) = -LogMath<T>::safeExp(
                        LogMath<T>::logDiv(logDeDy[k], logProb));
                }
            }
        } // if (!forwardOnly)
    }

    // return loss
    fblualib::luaPushTensor(L, losses);
    return 1;
}

template <class T>
int naiveDecoding(lua_State* L) {
    const thpp::Tensor<T>& input = fblualib::luaGetTensorChecked<T>(L, 1);
    const int nFrame = input.size(0);
    const int inputLength = input.size(1);

    thpp::Tensor<long> rawPred_ = input.max(2).second; // [nFrame x inputLength]
    thpp::Tensor<int> rawPred({nFrame, inputLength});
    rawPred.copy(rawPred_);

    thpp::Tensor<int> pred({nFrame, inputLength});
    pred.fill(0);

    for (int i = 0; i < nFrame; ++i) {
        std::vector<int> predVec;
        const int* rawPredData_i = rawPred[i].data();
        int* predData_i = pred[i].data();
        for (int t = 0; t < inputLength; ++t) {
            if (rawPredData_i[t] != 0 && !(t > 0 && rawPredData_i[t] == rawPredData_i[t-1])) {
                predVec.push_back(rawPredData_i[t]);
            }
        }
        for (int j = 0; j < (int)predVec.size(); ++j) {
            predData_i[j] = predVec[j];
        }
    }

    fblualib::luaPushTensor(L, pred);
    fblualib::luaPushTensor(L, rawPred);
    return 2;
}

// register functions
template <class T>
class Registerer {
private:
    static const luaL_Reg functions_[];
public:
    static void registerFunctions(lua_State* L);
};

template <class T>
const luaL_Reg Registerer<T>::functions_[] = {
    {"CTC_forwardBackward", forwardBackward<T>},
    {"CTC_naiveDecoding", naiveDecoding<T>},
    {nullptr, nullptr},
};

template <class T>
void Registerer<T>::registerFunctions(lua_State* L) {
    luaT_pushmetatable(L, thpp::Tensor<T>::kLuaTypeName);
    luaT_registeratname(L, functions_, "nn");
    lua_pop(L, 1);
}

} // namespace

void initCtc(lua_State* L) {
    Registerer<float>::registerFunctions(L);
    Registerer<double>::registerFunctions(L);
}
<|endoftext|>


<commit_before>#include "decoder.h"

#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <opencv2/core.hpp>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

namespace velodyne_puck {

using namespace sensor_msgs;
using namespace velodyne_msgs;

using Veckf = cv::Vec<float, Decoder::kChannels>;

/// Struct for precomputing sin and cos
struct SinCos {
  SinCos() = default;
  SinCos(float rad) : sin(std::sin(rad)), cos(std::cos(rad)) {}
  float sin, cos;
};

/// Convert image and camera_info to point cloud
CloudT::Ptr ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
                    bool organized);

Decoder::Decoder(const ros::NodeHandle& pnh)
    : pnh_(pnh), it_(pnh), cfg_server_(pnh) {
  pnh_.param<std::string>("frame_id", frame_id_, "velodyne");
  ROS_INFO("Velodyne frame_id: %s", frame_id_.c_str());
  cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));
}

Decoder::Decoded Decoder::DecodePacket(const Packet* packet,
                                       int64_t time) const {
  // Azimuth is clockwise, which is absurd
  // ^ y
  // | a /
  // |--/
  // | /
  // |/
  // o ------- > x

  // Check return mode and product id, for now just die
  const auto return_mode = packet->factory[0];
  if (!(return_mode == 55 || return_mode == 56)) {
    ROS_ERROR(
        "return mode must be Strongest (55) or Last Return (56), "
        "instead got (%u)",
        return_mode);
    ros::shutdown();
  }
  const auto product_id = packet->factory[1];
  if (product_id != 34) {
    ROS_ERROR("product id must be VLP-16 or Puck Lite (34), instead got (%u)",
              product_id);
    ros::shutdown();
  }

  // std::array<TimedFiringSequence, kFiringSequencesPerPacket>;
  Decoded decoded;
  // For each data block, 12 total
  for (int dbi = 0; dbi < kDataBlocksPerPacket; ++dbi) {
    const auto& block = packet->blocks[dbi];
    const auto raw_azimuth = block.azimuth;
    ROS_WARN_STREAM_COND(raw_azimuth > kMaxRawAzimuth,
                         "Invalid raw azimuth: " << raw_azimuth);
    ROS_WARN_COND(block.flag != UPPER_BANK, "Invalid block %d", dbi);

    // Fill in decoded
    // for each firing sequence in the data block, 2
    for (int fsi = 0; fsi < kFiringSequencesPerDataBlock; ++fsi) {
      // Index into decoded, hardcode 2 for now
      const auto di = dbi * 2 + fsi;
      FiringSequenceStamped& tfseq = decoded[di];
      // Assume all firings within each firing sequence occur at the same time
      tfseq.time = time + di * kFiringCycleNs;
      tfseq.azimuth = Raw2Azimuth(block.azimuth);  // need to fix half later
      tfseq.sequence = block.sequences[fsi];
    }
  }

  // Fix azimuth for odd firing sequences
  for (int dbi = 0; dbi < kDataBlocksPerPacket; ++dbi) {
    // 1,3,5,...,23
    // Index into decoded with odd id, hardcode 2 for now
    const auto di = dbi * 2 + 1;
    auto azimuth = decoded[di].azimuth;

    auto prev = di - 1;
    auto next = di + 1;
    // Handle last block where there's no next to inerpolate
    // Just use the previous two
    if (dbi == kDataBlocksPerPacket - 1) {
      prev -= 2;
      next -= 2;
    }

    auto azimuth_prev = decoded[prev].azimuth;
    auto azimuth_next = decoded[next].azimuth;

    // Handle angle warping
    // Based on the fact that all raw azimuth is within 0 to 2pi
    if (azimuth_next < azimuth_prev) {
      azimuth_next += kTau;
    }

    ROS_WARN_COND(azimuth_prev > azimuth_next,
                  "azimuth_prev %f > azimuth_next %f", azimuth_prev,
                  azimuth_next);

    const auto azimuth_diff = (azimuth_next - azimuth_prev) / 2.0;

    azimuth += azimuth_diff;
    if (azimuth > kTau) {
      azimuth -= kTau;
    }

    decoded[di].azimuth = azimuth;
  }

  return decoded;
}

void Decoder::PacketCb(const VelodynePacketConstPtr& packet_msg) {
  const auto* my_packet =
      reinterpret_cast<const Packet*>(&(packet_msg->data[0]));

  // TODO: maybe make this a vector? to handle invalid data block?
  const auto decoded = DecodePacket(my_packet, packet_msg->stamp.toNSec());

  if (!config_.full_sweep) {
    for (const auto& tfseq : decoded) {
      buffer_.push_back(tfseq);
      if (buffer_.size() >= static_cast<size_t>(config_.image_width)) {
        ROS_DEBUG("Publish fixed width with buffer size: %zu, required: %d",
                  buffer_.size(), config_.image_width);
        PublishBufferAndClear();
      }
    }
  } else {
    // Full scan mode 0~360
    // Check for a change of azimuth across 0
    float prev_azimuth = buffer_.empty() ? -1 : buffer_.back().azimuth;

    for (const auto& tfseq : decoded) {
      if (tfseq.azimuth < prev_azimuth) {
        // this indicates we cross the 0 azimuth angle
        // we are ready to publish what's in the buffer
        ROS_DEBUG("curr_azimuth: %f < %f prev_azimuth", rad2deg(tfseq.azimuth),
                  rad2deg(prev_azimuth));
        ROS_DEBUG("Publish full scan with buffer size: %zu", buffer_.size());
        PublishBufferAndClear();
      }

      buffer_.push_back(tfseq);
      prev_azimuth = tfseq.azimuth;
    }
  }
}

void Decoder::ScanCb(const VelodyneScanConstPtr& scan_msg) {
  // Not implemented at the moment
}

void Decoder::ConfigCb(VelodynePuckConfig& config, int level) {
  if (config.min_range > config.max_range) {
    ROS_WARN("min_range: %f > max_range: %f", config.min_range,
             config.max_range);
    config.min_range = config.max_range;
  }

  ROS_INFO(
      "Reconfigure Request: min_range: %f, max_range: %f, image_width: %d, "
      "organized: %s, full_sweep: %s",
      config.min_range, config.max_range, config.image_width,
      config.organized ? "True" : "False",
      config.full_sweep ? "True" : "False");

  config_ = config;
  buffer_.clear();

  if (level == -1) {
    ROS_INFO("Initialize ROS subscriber/publisher");
    cloud_pub_ = pnh_.advertise<PointCloud2>("cloud", 10);
    intensity_pub_ = it_.advertise("intensity", 1);
    camera_pub_ = it_.advertiseCamera("image", 5);

    packet_sub_ =
        pnh_.subscribe<VelodynePacket>("packet", 100, &Decoder::PacketCb, this);
    ROS_INFO("Ready to publish");
  }
}

void Decoder::PublishBufferAndClear() {
  if (buffer_.empty()) return;

  const auto start = ros::Time::now();
  // Always convert to image data
  const CameraInfoPtr cinfo_msg(new CameraInfo);
  const auto image_msg = ToImage(buffer_, *cinfo_msg);

  if (camera_pub_.getNumSubscribers()) {
    camera_pub_.publish(image_msg, cinfo_msg);
  }

  if (intensity_pub_.getNumSubscribers()) {
    PublishIntensity(image_msg);
  }

  if (cloud_pub_.getNumSubscribers()) {
    PublishCloud(image_msg, cinfo_msg);
  }

  buffer_.clear();

  const auto time = (ros::Time::now() - start).toSec();
  ROS_DEBUG("Total time for publish: %f", time);
}

void Decoder::PublishIntensity(const ImageConstPtr& image_msg) {
  const auto image = cv_bridge::toCvShare(image_msg)->image;
  cv::Mat intensity;
  cv::extractChannel(image, intensity, 1);
  intensity.convertTo(intensity, CV_8UC1);
  intensity_pub_.publish(
      cv_bridge::CvImage(image_msg->header, "mono8", intensity).toImageMsg());
}

void Decoder::PublishCloud(const ImageConstPtr& image_msg,
                           const CameraInfoConstPtr& cinfo_msg) {
  const auto cloud = ToCloud(image_msg, *cinfo_msg, config_.organized);
  ROS_DEBUG("number of points in cloud: %zu", cloud->size());
  cloud_pub_.publish(cloud);
}

ImagePtr Decoder::ToImage(const std::vector<FiringSequenceStamped>& fseqs,
                          CameraInfo& cinfo_msg) const {
  std_msgs::Header header;
  header.stamp.fromNSec(fseqs.front().time);
  header.frame_id = frame_id_;

  cv::Mat image = cv::Mat::zeros(kFiringsPerFiringSequence, fseqs.size(),
                                 CV_32FC(kChannels));
  ROS_DEBUG("image: %d x %d x %d", image.rows, image.cols, image.channels());

  cinfo_msg.header = header;
  cinfo_msg.height = image.rows;
  cinfo_msg.width = image.cols;
  cinfo_msg.K[0] = kMinElevation;
  cinfo_msg.K[1] = kMaxElevation;
  cinfo_msg.R[0] = kDistanceResolution;
  cinfo_msg.P[0] = kFiringCycleNs;   // ns
  cinfo_msg.P[1] = kSingleFiringNs;  // ns
  cinfo_msg.distortion_model = "VLP16";
  cinfo_msg.D.reserve(image.cols);

  // Unfortunately the buffer element is organized in columns, probably not very
  // cache-friendly
  for (int c = 0; c < image.cols; ++c) {
    const auto& tfseq = fseqs[c];
    // D stores each azimuth angle
    cinfo_msg.D.push_back(tfseq.azimuth);

    // Fill in image
    for (int r = 0; r < image.rows; ++r) {
      // row 0 corresponds to max elevation (highest), row 15 corresponds to
      // min elevation (lowest) hence we flip row number
      // also data points are stored in laser ids which are interleaved, so we
      // need to convert to index first. See p54 table
      const auto rr = Index2LaserId(kFiringsPerFiringSequence - 1 - r);

      // We clip range in image instead of in cloud
      float range = tfseq.sequence.points[rr].distance * kDistanceResolution;
      if (range < config_.min_range || range > config_.max_range) {
        range = kNaNFloat;
      }

      auto& e = image.at<Veckf>(r, c);
      e[0] = range;                                   // range
      e[1] = tfseq.sequence.points[rr].reflectivity;  // intensity
    }
  }

  return cv_bridge::CvImage(header, "32FC" + std::to_string(kChannels), image)
      .toImageMsg();
}

CloudT::Ptr ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
                    bool organized) {
  CloudT::Ptr cloud_ptr(new CloudT);
  CloudT& cloud = *cloud_ptr;

  const auto image = cv_bridge::toCvShare(image_msg)->image;
  const auto& azimuths = cinfo_msg.D;

  const float min_elevation = cinfo_msg.K[0];
  const float max_elevation = cinfo_msg.K[1];
  const float delta_elevation =
      (max_elevation - min_elevation) / (image.rows - 1);

  // Precompute sin cos
  std::vector<SinCos> sincos;
  sincos.reserve(azimuths.size());
  for (const auto& a : azimuths) {
    sincos.emplace_back(a);
  }

  cloud.header = pcl_conversions::toPCL(image_msg->header);
  cloud.reserve(image.total());

  for (int r = 0; r < image.rows; ++r) {
    const auto* const row_ptr = image.ptr<Veckf>(r);
    // Because image row 0 is the highest laser point
    const float omega = max_elevation - r * delta_elevation;
    const auto cos_omega = std::cos(omega);
    const auto sin_omega = std::sin(omega);

    for (int c = 0; c < image.cols; ++c) {
      const Veckf& data = row_ptr[c];

      PointT p;
      if (std::isnan(data[0])) {
        if (organized) {
          p.x = p.y = p.z = kNaNFloat;
          cloud.points.push_back(p);
        }
      } else {
        // p.53 Figure 9-1 VLP-16 Sensor Coordinate System
        // x = d * cos(w) * sin(a);
        // y = d * cos(w) * cos(a);
        // z = d * sin(w)
        const float R = data[0];
        const auto x = R * cos_omega * sincos[c].sin;
        const auto y = R * cos_omega * sincos[c].cos;
        const auto z = R * sin_omega;

        // original velodyne frame is x right y forward
        // we make x forward and y left, thus 0 azimuth is at x = 0 and
        // goes clockwise
        p.x = y;
        p.y = -x;
        p.z = z;
        p.intensity = data[1];

        cloud.points.push_back(p);
      }
    }
  }

  if (organized) {
    cloud.width = image.cols;
    cloud.height = image.rows;
  } else {
    cloud.width = cloud.size();
    cloud.height = 1;
  }

  return cloud_ptr;
}

}  // namespace velodyne_puck

int main(int argc, char** argv) {
  ros::init(argc, argv, "velodyne_puck_decoder");
  ros::NodeHandle pnh("~");

  velodyne_puck::Decoder node(pnh);
  ros::spin();
}
<commit_msg>add a debug statement<commit_after>#include "decoder.h"

#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <opencv2/core.hpp>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

namespace velodyne_puck {

using namespace sensor_msgs;
using namespace velodyne_msgs;

using Veckf = cv::Vec<float, Decoder::kChannels>;

/// Struct for precomputing sin and cos
struct SinCos {
  SinCos() = default;
  SinCos(float rad) : sin(std::sin(rad)), cos(std::cos(rad)) {}
  float sin, cos;
};

/// Convert image and camera_info to point cloud
CloudT::Ptr ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
                    bool organized);

Decoder::Decoder(const ros::NodeHandle& pnh)
    : pnh_(pnh), it_(pnh), cfg_server_(pnh) {
  pnh_.param<std::string>("frame_id", frame_id_, "velodyne");
  ROS_INFO("Velodyne frame_id: %s", frame_id_.c_str());
  cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));
}

Decoder::Decoded Decoder::DecodePacket(const Packet* packet,
                                       int64_t time) const {
  // Azimuth is clockwise, which is absurd
  // ^ y
  // | a /
  // |--/
  // | /
  // |/
  // o ------- > x

  // Check return mode and product id, for now just die
  const auto return_mode = packet->factory[0];
  if (!(return_mode == 55 || return_mode == 56)) {
    ROS_ERROR(
        "return mode must be Strongest (55) or Last Return (56), "
        "instead got (%u)",
        return_mode);
    ros::shutdown();
  }
  const auto product_id = packet->factory[1];
  if (product_id != 34) {
    ROS_ERROR("product id must be VLP-16 or Puck Lite (34), instead got (%u)",
              product_id);
    ros::shutdown();
  }

  // std::array<TimedFiringSequence, kFiringSequencesPerPacket>;
  Decoded decoded;
  // For each data block, 12 total
  for (int dbi = 0; dbi < kDataBlocksPerPacket; ++dbi) {
    const auto& block = packet->blocks[dbi];
    const auto raw_azimuth = block.azimuth;
    ROS_WARN_STREAM_COND(raw_azimuth > kMaxRawAzimuth,
                         "Invalid raw azimuth: " << raw_azimuth);
    ROS_WARN_COND(block.flag != UPPER_BANK, "Invalid block %d", dbi);

    // Fill in decoded
    // for each firing sequence in the data block, 2
    for (int fsi = 0; fsi < kFiringSequencesPerDataBlock; ++fsi) {
      // Index into decoded, hardcode 2 for now
      const auto di = dbi * 2 + fsi;
      FiringSequenceStamped& tfseq = decoded[di];
      // Assume all firings within each firing sequence occur at the same time
      tfseq.time = time + di * kFiringCycleNs;
      tfseq.azimuth = Raw2Azimuth(block.azimuth);  // need to fix half later
      tfseq.sequence = block.sequences[fsi];
    }
  }

  // Fix azimuth for odd firing sequences
  for (int dbi = 0; dbi < kDataBlocksPerPacket; ++dbi) {
    // 1,3,5,...,23
    // Index into decoded with odd id, hardcode 2 for now
    const auto di = dbi * 2 + 1;
    auto azimuth = decoded[di].azimuth;

    auto prev = di - 1;
    auto next = di + 1;
    // Handle last block where there's no next to inerpolate
    // Just use the previous two
    if (dbi == kDataBlocksPerPacket - 1) {
      prev -= 2;
      next -= 2;
    }

    auto azimuth_prev = decoded[prev].azimuth;
    auto azimuth_next = decoded[next].azimuth;

    // Handle angle warping
    // Based on the fact that all raw azimuth is within 0 to 2pi
    if (azimuth_next < azimuth_prev) {
      azimuth_next += kTau;
    }

    ROS_WARN_COND(azimuth_prev > azimuth_next,
                  "azimuth_prev %f > azimuth_next %f", azimuth_prev,
                  azimuth_next);

    const auto azimuth_diff = (azimuth_next - azimuth_prev) / 2.0;

    azimuth += azimuth_diff;
    if (azimuth > kTau) {
      azimuth -= kTau;
    }

    decoded[di].azimuth = azimuth;
  }

  return decoded;
}

void Decoder::PacketCb(const VelodynePacketConstPtr& packet_msg) {
  const auto* my_packet =
      reinterpret_cast<const Packet*>(&(packet_msg->data[0]));

  // TODO: maybe make this a vector? to handle invalid data block?
  const auto decoded = DecodePacket(my_packet, packet_msg->stamp.toNSec());

  if (!config_.full_sweep) {
    for (const auto& tfseq : decoded) {
      buffer_.push_back(tfseq);
      if (buffer_.size() >= static_cast<size_t>(config_.image_width)) {
        ROS_DEBUG("Publish fixed width with buffer size: %zu, required: %d",
                  buffer_.size(), config_.image_width);
        PublishBufferAndClear();
      }
    }
  } else {
    // Full scan mode 0~360
    // Check for a change of azimuth across 0
    float prev_azimuth = buffer_.empty() ? -1 : buffer_.back().azimuth;

    for (const auto& tfseq : decoded) {
      if (tfseq.azimuth < prev_azimuth) {
        // this indicates we cross the 0 azimuth angle
        // we are ready to publish what's in the buffer
        ROS_DEBUG("curr_azimuth: %f < %f prev_azimuth", rad2deg(tfseq.azimuth),
                  rad2deg(prev_azimuth));
        ROS_DEBUG("Publish full scan with buffer size: %zu", buffer_.size());
        PublishBufferAndClear();
      }

      buffer_.push_back(tfseq);
      prev_azimuth = tfseq.azimuth;
    }
  }
}

void Decoder::ScanCb(const VelodyneScanConstPtr& scan_msg) {
  // Not implemented at the moment
}

void Decoder::ConfigCb(VelodynePuckConfig& config, int level) {
  if (config.min_range > config.max_range) {
    ROS_WARN("min_range: %f > max_range: %f", config.min_range,
             config.max_range);
    config.min_range = config.max_range;
  }

  ROS_INFO(
      "Reconfigure Request: min_range: %f, max_range: %f, image_width: %d, "
      "organized: %s, full_sweep: %s",
      config.min_range, config.max_range, config.image_width,
      config.organized ? "True" : "False",
      config.full_sweep ? "True" : "False");

  config_ = config;
  buffer_.clear();

  if (level == -1) {
    ROS_INFO("Initialize ROS subscriber/publisher");
    cloud_pub_ = pnh_.advertise<PointCloud2>("cloud", 10);
    intensity_pub_ = it_.advertise("intensity", 1);
    camera_pub_ = it_.advertiseCamera("image", 5);

    packet_sub_ =
        pnh_.subscribe<VelodynePacket>("packet", 100, &Decoder::PacketCb, this);
    ROS_INFO("Ready to publish");
  }
}

void Decoder::PublishBufferAndClear() {
  if (buffer_.empty()) return;

  const auto start = ros::Time::now();
  // Always convert to image data
  const CameraInfoPtr cinfo_msg(new CameraInfo);
  const auto image_msg = ToImage(buffer_, *cinfo_msg);

  if (camera_pub_.getNumSubscribers()) {
    camera_pub_.publish(image_msg, cinfo_msg);
  }

  if (intensity_pub_.getNumSubscribers()) {
    PublishIntensity(image_msg);
  }

  if (cloud_pub_.getNumSubscribers()) {
    PublishCloud(image_msg, cinfo_msg);
  }

  ROS_DEBUG("Clearing buffer %zu", buffer_.size());
  buffer_.clear();

  const auto time = (ros::Time::now() - start).toSec();
  ROS_DEBUG("Total time for publish: %f", time);
}

void Decoder::PublishIntensity(const ImageConstPtr& image_msg) {
  const auto image = cv_bridge::toCvShare(image_msg)->image;
  cv::Mat intensity;
  cv::extractChannel(image, intensity, 1);
  intensity.convertTo(intensity, CV_8UC1);
  intensity_pub_.publish(
      cv_bridge::CvImage(image_msg->header, "mono8", intensity).toImageMsg());
}

void Decoder::PublishCloud(const ImageConstPtr& image_msg,
                           const CameraInfoConstPtr& cinfo_msg) {
  const auto cloud = ToCloud(image_msg, *cinfo_msg, config_.organized);
  ROS_DEBUG("number of points in cloud: %zu", cloud->size());
  cloud_pub_.publish(cloud);
}

ImagePtr Decoder::ToImage(const std::vector<FiringSequenceStamped>& fseqs,
                          CameraInfo& cinfo_msg) const {
  std_msgs::Header header;
  header.stamp.fromNSec(fseqs.front().time);
  header.frame_id = frame_id_;

  cv::Mat image = cv::Mat::zeros(kFiringsPerFiringSequence, fseqs.size(),
                                 CV_32FC(kChannels));
  ROS_DEBUG("image: %d x %d x %d", image.rows, image.cols, image.channels());

  cinfo_msg.header = header;
  cinfo_msg.height = image.rows;
  cinfo_msg.width = image.cols;
  cinfo_msg.K[0] = kMinElevation;
  cinfo_msg.K[1] = kMaxElevation;
  cinfo_msg.R[0] = kDistanceResolution;
  cinfo_msg.P[0] = kFiringCycleNs;   // ns
  cinfo_msg.P[1] = kSingleFiringNs;  // ns
  cinfo_msg.distortion_model = "VLP16";
  cinfo_msg.D.reserve(image.cols);

  // Unfortunately the buffer element is organized in columns, probably not very
  // cache-friendly
  for (int c = 0; c < image.cols; ++c) {
    const auto& tfseq = fseqs[c];
    // D stores each azimuth angle
    cinfo_msg.D.push_back(tfseq.azimuth);

    // Fill in image
    for (int r = 0; r < image.rows; ++r) {
      // row 0 corresponds to max elevation (highest), row 15 corresponds to
      // min elevation (lowest) hence we flip row number
      // also data points are stored in laser ids which are interleaved, so we
      // need to convert to index first. See p54 table
      const auto rr = Index2LaserId(kFiringsPerFiringSequence - 1 - r);

      // We clip range in image instead of in cloud
      float range = tfseq.sequence.points[rr].distance * kDistanceResolution;
      if (range < config_.min_range || range > config_.max_range) {
        range = kNaNFloat;
      }

      auto& e = image.at<Veckf>(r, c);
      e[0] = range;                                   // range
      e[1] = tfseq.sequence.points[rr].reflectivity;  // intensity
    }
  }

  return cv_bridge::CvImage(header, "32FC" + std::to_string(kChannels), image)
      .toImageMsg();
}

CloudT::Ptr ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
                    bool organized) {
  CloudT::Ptr cloud_ptr(new CloudT);
  CloudT& cloud = *cloud_ptr;

  const auto image = cv_bridge::toCvShare(image_msg)->image;
  const auto& azimuths = cinfo_msg.D;

  const float min_elevation = cinfo_msg.K[0];
  const float max_elevation = cinfo_msg.K[1];
  const float delta_elevation =
      (max_elevation - min_elevation) / (image.rows - 1);

  // Precompute sin cos
  std::vector<SinCos> sincos;
  sincos.reserve(azimuths.size());
  for (const auto& a : azimuths) {
    sincos.emplace_back(a);
  }

  cloud.header = pcl_conversions::toPCL(image_msg->header);
  cloud.reserve(image.total());

  for (int r = 0; r < image.rows; ++r) {
    const auto* const row_ptr = image.ptr<Veckf>(r);
    // Because image row 0 is the highest laser point
    const float omega = max_elevation - r * delta_elevation;
    const auto cos_omega = std::cos(omega);
    const auto sin_omega = std::sin(omega);

    for (int c = 0; c < image.cols; ++c) {
      const Veckf& data = row_ptr[c];

      PointT p;
      if (std::isnan(data[0])) {
        if (organized) {
          p.x = p.y = p.z = kNaNFloat;
          cloud.points.push_back(p);
        }
      } else {
        // p.53 Figure 9-1 VLP-16 Sensor Coordinate System
        // x = d * cos(w) * sin(a);
        // y = d * cos(w) * cos(a);
        // z = d * sin(w)
        const float R = data[0];
        const auto x = R * cos_omega * sincos[c].sin;
        const auto y = R * cos_omega * sincos[c].cos;
        const auto z = R * sin_omega;

        // original velodyne frame is x right y forward
        // we make x forward and y left, thus 0 azimuth is at x = 0 and
        // goes clockwise
        p.x = y;
        p.y = -x;
        p.z = z;
        p.intensity = data[1];

        cloud.points.push_back(p);
      }
    }
  }

  if (organized) {
    cloud.width = image.cols;
    cloud.height = image.rows;
  } else {
    cloud.width = cloud.size();
    cloud.height = 1;
  }

  return cloud_ptr;
}

}  // namespace velodyne_puck

int main(int argc, char** argv) {
  ros::init(argc, argv, "velodyne_puck_decoder");
  ros::NodeHandle pnh("~");

  velodyne_puck::Decoder node(pnh);
  ros::spin();
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) Jason White
 *
 * MIT License
 *
 * Description:
 * Path manipulation module.
 */
#include "embedded.h"

#include <string.h> // for strcmp
#include <stdlib.h> // for bsearch

#include <lua.hpp>

// Helper macro for adding new modules
#define SCRIPT(module, path, name) \
    {(module), (path), scripts_ ## name ## _lua, scripts_ ## name ## _lua_len}

namespace {

struct Script
{
    const char* name;
    const char* path;
    const void* data;
    size_t length;

    // Loads this Lua script
    int load(lua_State* L) const;
};

/**
 * Main scripts
 */
#include "embedded/init.c"
#include "embedded/shutdown.c"

/**
 * Initialization/shutdown scripts.
 */
const Script script_init     = SCRIPT("init", "init.lua", init);
const Script script_shutdown = SCRIPT("shutdown", "shutdown.lua", shutdown);

/**
 * Modules to embed
 */
#include "embedded/rules.c"
#include "embedded/rules/cc.c"
#include "embedded/rules/cc/gcc.c"
#include "embedded/rules/d.c"
#include "embedded/rules/d/dmd.c"

/**
 * List of embedded Lua scripts.
 *
 * NOTE: This must be in alphabetical order according to the Lua script path.
 */
const Script embedded[] = {
    SCRIPT("rules", "{embedded}/rules.lua", rules),
    SCRIPT("rules.cc", "{embedded}/rules/cc.lua", rules_cc),
    SCRIPT("rules.cc.gcc", "{embedded}/rules/cc/gcc.lua", rules_cc_gcc),
    SCRIPT("rules.d", "{embedded}/rules/d.lua", rules_d),
    SCRIPT("rules.d.dmd", "{embedded}/rules/d/dmd.lua", rules_d_dmd),
};

const size_t embedded_len = sizeof(embedded)/sizeof(Script);

int compare_embedded(const void* key, const void* elem) {
    return strcmp((const char*)key, ((const Script*)elem)->name);
}

// Note that this assumes the list of modules is sorted.
const Script* find_embedded(const char* name) {
    return (const Script*)bsearch(name, embedded, embedded_len, sizeof(Script), compare_embedded);
}

int Script::load(lua_State* L) const {
    return luaL_loadbuffer(L, (const char*)data, length, name);
}

} // namespace

int load_embedded(lua_State* L, const char* name)
{
    const Script* m = find_embedded(name);

    if (!m) {
        lua_pushfstring(L, "embedded script '%s' not found", name);
        return LUA_ERRFILE;
    }

    return m->load(L);
}

int embedded_searcher(lua_State* L) {
    const char* name = luaL_checkstring(L, 1);

    const Script* m = find_embedded(name);

    if (!m) {
        lua_pushfstring(L, "embedded script '%s' not found", name);
        return 1;
    }

    // Return block function + file name to pass to it
    if (m->load(L))
        return 1;

    lua_pushstring(L, m->path);
    return 2;
}

int load_init(lua_State* L) {
    return script_init.load(L);
}

int load_shutdown(lua_State* L) {
    return script_shutdown.load(L);
}
<commit_msg>Fix consumption of embedded scripts<commit_after>/**
 * Copyright (c) Jason White
 *
 * MIT License
 *
 * Description:
 * Path manipulation module.
 */
#include "embedded.h"

#include <string.h> // for strcmp
#include <stdlib.h> // for bsearch

#include <lua.hpp>

// Helper macro for adding new modules
#define SCRIPT(module, path, name) \
    {(module), (path), scripts_ ## name ## _lua, sizeof(scripts_ ## name ## _lua)}

namespace {

struct Script
{
    const char* name;
    const char* path;
    const void* data;
    size_t length;

    // Loads this Lua script
    int load(lua_State* L) const;
};

/**
 * Main scripts
 */
#include "embedded/init.c"
#include "embedded/shutdown.c"

/**
 * Initialization/shutdown scripts.
 */
const Script script_init     = SCRIPT("init", "init.lua", init);
const Script script_shutdown = SCRIPT("shutdown", "shutdown.lua", shutdown);

/**
 * Modules to embed
 */
#include "embedded/rules.c"
#include "embedded/rules/cc.c"
#include "embedded/rules/cc/gcc.c"
#include "embedded/rules/d.c"
#include "embedded/rules/d/dmd.c"

/**
 * List of embedded Lua scripts.
 *
 * NOTE: This must be in alphabetical order according to the Lua script path.
 */
const Script embedded[] = {
    SCRIPT("rules", "{embedded}/rules.lua", rules),
    SCRIPT("rules.cc", "{embedded}/rules/cc.lua", rules_cc),
    SCRIPT("rules.cc.gcc", "{embedded}/rules/cc/gcc.lua", rules_cc_gcc),
    SCRIPT("rules.d", "{embedded}/rules/d.lua", rules_d),
    SCRIPT("rules.d.dmd", "{embedded}/rules/d/dmd.lua", rules_d_dmd),
};

const size_t embedded_len = sizeof(embedded)/sizeof(Script);

int compare_embedded(const void* key, const void* elem) {
    return strcmp((const char*)key, ((const Script*)elem)->name);
}

// Note that this assumes the list of modules is sorted.
const Script* find_embedded(const char* name) {
    return (const Script*)bsearch(name, embedded, embedded_len, sizeof(Script), compare_embedded);
}

int Script::load(lua_State* L) const {
    return luaL_loadbuffer(L, (const char*)data, length, name);
}

} // namespace

int load_embedded(lua_State* L, const char* name)
{
    const Script* m = find_embedded(name);

    if (!m) {
        lua_pushfstring(L, "embedded script '%s' not found", name);
        return LUA_ERRFILE;
    }

    return m->load(L);
}

int embedded_searcher(lua_State* L) {
    const char* name = luaL_checkstring(L, 1);

    const Script* m = find_embedded(name);

    if (!m) {
        lua_pushfstring(L, "embedded script '%s' not found", name);
        return 1;
    }

    // Return block function + file name to pass to it
    if (m->load(L))
        return 1;

    lua_pushstring(L, m->path);
    return 2;
}

int load_init(lua_State* L) {
    return script_init.load(L);
}

int load_shutdown(lua_State* L) {
    return script_shutdown.load(L);
}
<|endoftext|>


<commit_before><commit_msg>Update lin_search.cpp<commit_after><|endoftext|>


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

#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_

#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/common/point_operators.h>
#include <pcl/common/eigen.h>

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
{
  using namespace pcl::common;
  using namespace pcl::traits;

  PointT p10 = input_->points[samples[1]] - input_->points[samples[0]];
  PointT p20 = input_->points[samples[2]] - input_->points[samples[0]];
  PointT p21 = input_->points[samples[2]] - input_->points[samples[1]];

  return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
          (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
          (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
    return (false);
  }
  // Need 3 samples
  if (samples.size () != 3)
    return (false);

  std::vector<int> indices_tgt (3);
  for (int i = 0; i < 3; ++i)
    indices_tgt[i] = correspondences_[samples[i]];

  estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    distances.clear ();
    return;
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
    return;
  }
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }
  distances.resize (indices_->size ());

  // Get the 4x4 transformation
  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);
    // Calculate the distance from the transformed point to its correspondence
    // need to compute the real norm here to keep MSAC and friends general
    distances[i] = (p_tr - pt_tgt).norm ();
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    inliers.clear ();
    return;
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
    return;
  }

  double thresh = threshold * threshold;

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    inliers.clear ();
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);

    float distance = (p_tr - pt_tgt).squaredNorm ();
    // Calculate the distance from the transformed point to its correspondence
    if (distance < thresh)
    {
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = static_cast<double> (distance);
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelRegistration<PointT>::countWithinDistance (
    const Eigen::VectorXf &model_coefficients, const double threshold)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    return (0);
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
    return (0);
  }

  double thresh = threshold * threshold;

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
    return (0);

  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  int nr_p = 0;
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);
    // Calculate the distance from the transformed point to its correspondence
    if ((p_tr - pt_tgt).squaredNorm () < thresh)
      nr_p++;
  }
  return (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients) || !target_)
  {
    optimized_coefficients = model_coefficients;
    return;
  }

  std::vector<int> indices_src (inliers.size ());
  std::vector<int> indices_tgt (inliers.size ());
  for (size_t i = 0; i < inliers.size (); ++i)
  {
    // NOTE: not tested!
    indices_src[i] = (*indices_)[inliers[i]];
    indices_tgt[i] = (*indices_tgt_)[inliers[i]];
  }

  estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
    const pcl::PointCloud<PointT> &cloud_src,
    const std::vector<int> &indices_src,
    const pcl::PointCloud<PointT> &cloud_tgt,
    const std::vector<int> &indices_tgt,
    Eigen::VectorXf &transform)
{
  transform.resize (16);

  Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());

  for (size_t i = 0; i < indices_src.size (); ++i)
  {
    src (0, i) = cloud_src[indices_src[i]].x;
    src (1, i) = cloud_src[indices_src[i]].y;
    src (2, i) = cloud_src[indices_src[i]].z;

    tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
    tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
    tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
  }

  // Call Umeyama directly from Eigen
  Eigen::Matrix4d transformation_matrix = pcl::umeyama (src, tgt, false);

  // Return the correct transformation
  transform.segment<4> (0).matrix () = transformation_matrix.cast<float> ().row (0);
  transform.segment<4> (4).matrix () = transformation_matrix.cast<float> ().row (1);
  transform.segment<4> (8).matrix () = transformation_matrix.cast<float> ().row (2);
  transform.segment<4> (12).matrix () = transformation_matrix.cast<float> ().row (3);
}

#define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_

<commit_msg>* fixed a bug where the set of query/target indices were not correctly estimated in @pcl::SampleConsensusModelRegistration::optimizeModelCoefficients@<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_

#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/common/point_operators.h>
#include <pcl/common/eigen.h>

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
{
  using namespace pcl::common;
  using namespace pcl::traits;

  PointT p10 = input_->points[samples[1]] - input_->points[samples[0]];
  PointT p20 = input_->points[samples[2]] - input_->points[samples[0]];
  PointT p21 = input_->points[samples[2]] - input_->points[samples[1]];

  return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
          (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
          (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
    return (false);
  }
  // Need 3 samples
  if (samples.size () != 3)
    return (false);

  std::vector<int> indices_tgt (3);
  for (int i = 0; i < 3; ++i)
    indices_tgt[i] = correspondences_[samples[i]];

  estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    distances.clear ();
    return;
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
    return;
  }
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }
  distances.resize (indices_->size ());

  // Get the 4x4 transformation
  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);
    // Calculate the distance from the transformed point to its correspondence
    // need to compute the real norm here to keep MSAC and friends general
    distances[i] = (p_tr - pt_tgt).norm ();
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    inliers.clear ();
    return;
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
    return;
  }

  double thresh = threshold * threshold;

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    inliers.clear ();
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);

    float distance = (p_tr - pt_tgt).squaredNorm ();
    // Calculate the distance from the transformed point to its correspondence
    if (distance < thresh)
    {
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = static_cast<double> (distance);
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelRegistration<PointT>::countWithinDistance (
    const Eigen::VectorXf &model_coefficients, const double threshold)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    return (0);
  }
  if (!target_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
    return (0);
  }

  double thresh = threshold * threshold;

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
    return (0);

  Eigen::Matrix4f transform;
  transform.row (0).matrix () = model_coefficients.segment<4>(0);
  transform.row (1).matrix () = model_coefficients.segment<4>(4);
  transform.row (2).matrix () = model_coefficients.segment<4>(8);
  transform.row (3).matrix () = model_coefficients.segment<4>(12);

  int nr_p = 0;
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
                            input_->points[(*indices_)[i]].y,
                            input_->points[(*indices_)[i]].z, 1);
    Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
                            target_->points[(*indices_tgt_)[i]].y,
                            target_->points[(*indices_tgt_)[i]].z, 1);

    Eigen::Vector4f p_tr (transform * pt_src);
    // Calculate the distance from the transformed point to its correspondence
    if ((p_tr - pt_tgt).squaredNorm () < thresh)
      nr_p++;
  }
  return (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
  if (indices_->size () != indices_tgt_->size ())
  {
    PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients) || !target_)
  {
    optimized_coefficients = model_coefficients;
    return;
  }

  std::vector<int> indices_src (inliers.size ());
  std::vector<int> indices_tgt (inliers.size ());
  for (size_t i = 0; i < inliers.size (); ++i)
  {
    // NOTE: not tested!
    indices_src[i] = inliers[i];
    indices_tgt[i] = correspondences_[indices_src[i]];
  }

  estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
    const pcl::PointCloud<PointT> &cloud_src,
    const std::vector<int> &indices_src,
    const pcl::PointCloud<PointT> &cloud_tgt,
    const std::vector<int> &indices_tgt,
    Eigen::VectorXf &transform)
{
  transform.resize (16);

  Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());

  for (size_t i = 0; i < indices_src.size (); ++i)
  {
    src (0, i) = cloud_src[indices_src[i]].x;
    src (1, i) = cloud_src[indices_src[i]].y;
    src (2, i) = cloud_src[indices_src[i]].z;

    tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
    tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
    tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
  }

  // Call Umeyama directly from Eigen
  Eigen::Matrix4d transformation_matrix = pcl::umeyama (src, tgt, false);

  // Return the correct transformation
  transform.segment<4> (0).matrix () = transformation_matrix.cast<float> ().row (0);
  transform.segment<4> (4).matrix () = transformation_matrix.cast<float> ().row (1);
  transform.segment<4> (8).matrix () = transformation_matrix.cast<float> ().row (2);
  transform.segment<4> (12).matrix () = transformation_matrix.cast<float> ().row (3);
}

#define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_

<|endoftext|>