utest.h

Go to the documentation of this file.

#ifndef UTEST_HPP_
#define UTEST_HPP_

#include "pointmatcher/PointMatcher.h"
#include "pointmatcher/IO.h"
#include "../contrib/gtest/gtest.h"

#include <string>
#include <fstream>

#include "boost/filesystem.hpp"
#include "boost/filesystem/path.hpp"
#include "boost/filesystem/operations.hpp"

typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;

extern std::string dataPath;

extern DP ref2D;
extern DP data2D;
extern DP ref3D;
extern DP data3D;
extern PM::TransformationParameters validT2d;
extern PM::TransformationParameters validT3d;

//---------------------------
// Base for ICP tests
//---------------------------

class IcpHelper: public testing::Test
{
public:
        
        PM::ICP icp;
        
        PM::Parameters params;

        virtual void dumpVTK()
        {
                // Make available a VTK inspector for manual inspection
                icp.inspector.reset(
                        PM::get().InspectorRegistrar.create(
                                "VTKFileInspector", 
                                boost::assign::map_list_of
                                        ("baseFileName","./unitTest")
                        )
                );
        }
        
        void validate2dTransformation()
        {
                const PM::TransformationParameters testT = icp(data2D, ref2D);
                const int dim = validT2d.cols();

                const BOOST_AUTO(validTrans, validT2d.block(0, dim-1, dim-1, 1).norm());
                const BOOST_AUTO(testTrans, testT.block(0, dim-1, dim-1, 1).norm());
        
                const BOOST_AUTO(validAngle, acos(validT2d(0,0)));
                const BOOST_AUTO(testAngle, acos(testT(0,0)));
                
                EXPECT_NEAR(validTrans, testTrans, 0.05);
                EXPECT_NEAR(validAngle, testAngle, 0.05);
        }

        void validate3dTransformation()
        {
                //dumpVTK();

                const PM::TransformationParameters testT = icp(data3D, ref3D);
                const int dim = validT2d.cols();

                const BOOST_AUTO(validTrans, validT3d.block(0, dim-1, dim-1, 1).norm());
                const BOOST_AUTO(testTrans, testT.block(0, dim-1, dim-1, 1).norm());
        
                const BOOST_AUTO(testRotation, Eigen::Quaternion<float>(Eigen::Matrix<float,3,3>(testT.topLeftCorner(3,3))));
                const BOOST_AUTO(validRotation, Eigen::Quaternion<float>(Eigen::Matrix<float,3,3>(validT3d.topLeftCorner(3,3))));
                
                const BOOST_AUTO(angleDist, validRotation.angularDistance(testRotation));
                
                //cout << testT << endl;
                //cout << "angleDist: " << angleDist << endl;
                //cout << "transDist: " << abs(validTrans-testTrans) << endl;
                EXPECT_NEAR(validTrans, testTrans, 0.1);
                EXPECT_NEAR(angleDist, 0.0, 0.1);

        }
};

#endif /* UTEST_HPP_ */