CoordinateTransform.cpp

Go to the documentation of this file.

/*
 * CoordinateTransform.cpp
 *
 *  Created on: 17.04.2012
 *      Author: Thomas Wiemann
 */

#include "lvr2/io/CoordinateTransform.hpp"
#include "lvr2/io/Timestamp.hpp"

#include <boost/shared_ptr.hpp>
#include <iostream>
using std::cout;
using std::endl;

namespace lvr2
{

void convert(COORD_SYSTEM from, COORD_SYSTEM to, float* point)
{
        if(from == OPENGL_METERS)
        {
                if(to == SLAM6D)
                {
                        float x = point[0];
                        float y = point[1];
                        float z = point[2];

                        point[0] = 100 * x;
                        point[1] = 100 * y;
                        point[2] = -100 * z;
                }
                else
                {
                        cout << timestamp << "Target coordinate system not supported." << endl;
                }
        }
        else
        {
                cout << timestamp << "Source coordinate system not supported." << endl;
        }
}

void convert(COORD_SYSTEM from, COORD_SYSTEM to, PointBufferPtr& buffer)
{
        size_t n = buffer->numPoints();
        floatArr p = buffer->getPointArray();
        for(size_t i = 0; i < n; i++)
        {
                int pos = 3 * i;
                float* point = &p[pos];
                convert(OPENGL_METERS, SLAM6D, point);
        }
}

} // namespace lvr2