Match3D.hpp

//
// COP match tool for SAL3D
// Copyright (C) 2008-2010 AQSENSE, S.L.
//
#if !defined (AQSENSE_SAL3DPP_MATCH3D_HPP)
#define AQSENSE_SAL3DPP_MATCH3D_HPP

#if defined (_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif // _MSC_VER && _MSC_VER >= 1020

#include <sal3d/match3d.h>
#include <sal3dpp/COP.hpp>
#include <vector>

namespace
{
    const int defaultMaxIterations = 100;
    const float defaultMaxTranslationError = 0.005f;
    const float defaultMaxRotationError = 0.001f;
    const bool defaultExtractFeatures = true;
}

namespace sal3d
{
    typedef sal3d_surface_point SurfacePoint;
    typedef std::vector<SurfacePoint> SurfacePoints;

    struct Match3DParameters
    {
        Match3DParameters():
            maxIterations(defaultMaxIterations),
            maxTranslationError(defaultMaxTranslationError),
            maxRotationError(defaultMaxRotationError),
            extractFeatures(defaultExtractFeatures)
        {}

        int maxIterations;

        float maxTranslationError;

        float maxRotationError;

        bool extractFeatures;
    };

    class Match3D
    {
        public:
            explicit Match3D (const sal3d::COP &cop)
            {
                _match3d.p = 0;
                Error e;
                sal3d_match3d_new_with_cop (cop.c_cop (), &_match3d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            explicit Match3D (const SurfacePoints &points)
            {
                _match3d.p = 0;
                Error e;
                sal3d_match3d_new_with_surface_points (&points[0], points.size(), &_match3d,
                        &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            Match3D (const Match3D &match3d):
                _match3d (match3d._match3d)
            {
                Error e;
                sal3d_match3d_share (_match3d, &e.e);
                if (e.e.value < 0)
                    throw e;
            }

            ~Match3D ()
            {
                sal3d_match3d_release (_match3d);
            }

            Movement3D
            operator() (const sal3d::COP &cop) const
            {
                float quality;
                int iters;
                Match3DParameters params;

                return this->operator()(cop, params, quality, iters);
            }

            Movement3D
            operator() (const sal3d::COP &cop, float &quality) const
            {
                int iters;
                Match3DParameters params;

                return this->operator()(cop, params, quality, iters);
            }

            Movement3D
            operator() (const sal3d::COP &cop, const Match3DParameters &params,
                    float &quality, int &iterations) const
            {
                sal3d_movement_3d movement;
                Error e;
                sal3d_match3d_align (_match3d, cop.c_cop (),
                        params.maxIterations, params.maxTranslationError,
                        params.maxRotationError, params.extractFeatures,
                        &movement, &quality, &iterations, &e.e);
                if (e.e.value < 0)
                    throw e;

                return Movement3D (movement);
            }

            Movement3D
            operator() (const sal3d::COP &cop,
                    const Movement3D &prealign) const
            {
                float quality;
                int iters;
                Match3DParameters params;

                return this->operator()(cop, prealign, params, quality, iters);
            }

            Movement3D
            operator() (const sal3d::COP &cop,
                    const Movement3D &prealign, float &quality) const
            {
                int iters;
                Match3DParameters params;

                return this->operator()(cop, prealign, params, quality, iters);
            }

            Movement3D
            operator() (const sal3d::COP &cop, const Movement3D &prealign,
                    const Match3DParameters &params, float &quality,
                    int &iterations) const
            {
                sal3d_movement_3d movement;
                Error e;
                sal3d_match3d_align_moved(_match3d, cop.c_cop(),
                        &prealign.c_movement_3d(), params.maxIterations,
                        params.maxTranslationError,
                        params.maxRotationError, params.extractFeatures,
                        &movement, &quality, &iterations, &e.e);

                if (e.e.value < 0)
                    throw e;

                return Movement3D (movement);
            }

            Match3D &
            operator= (const Match3D &rhs)
            {
                Error e;
                sal3d_match3d_share (rhs._match3d, &e.e);
                if (e.e.value < 0)
                    throw e;
                sal3d_match3d_release (_match3d);
                _match3d = rhs._match3d;

                return *this;
            }

        private:
            sal3d_match3d _match3d;
    };
}

#endif // !AQSENSE_SAL3DPP_MATCH3D_HPP