Container3d.h

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#ifndef CONTAINER_3D
#define CONTAINER_3D

#include "cv.h"
#include <utility>
#include <vector>
#include <algorithm>

namespace alvar {

template <class T> class Container3d;

template <class T>
class Container3dSortDist {
protected:
        CvPoint3D32f orig;
        Container3d<T> &container;
public:
        Container3dSortDist(Container3d<T> &_container, const CvPoint3D32f _orig) : container(_container), orig(_orig) {}
        bool operator()(size_t i1, size_t i2)
        {
                float x1 = container[i1].first.x-orig.x, x2 = container[i2].first.x-orig.x;
                float y1 = container[i1].first.y-orig.y, y2 = container[i2].first.y-orig.y;
                float z1 = container[i1].first.z-orig.z, z2 = container[i2].first.z-orig.z;
                float d1 = x1*x1 + y1*y1 + z1*z1;
                float d2 = x2*x2 + y2*y2 + z2*z2;
                return d1<d2;
        }
};

template <class T>
class Container3dSortSize {
protected:
        Container3d<T> &container;
public:
        Container3dSortSize(Container3d<T> &_container) : container(_container) {}
        bool operator()(size_t i1, size_t i2)
        {
                return (container[i1].second->size() > container[i2].second->size());
        }
};

template <class T>
class Container3dLimitDist {
protected:
        Container3d<T> &container;
        CvPoint3D32f orig;
        float dist_limit;
public:
        Container3dLimitDist(Container3d<T> &_container, const CvPoint3D32f _orig, float _dist_limit) 
                : container(_container),orig(_orig),dist_limit(_dist_limit) {}
        bool operator()(size_t i) const {
                float x = container[i].first.x-orig.x;
                float y = container[i].first.y-orig.y;
                float z = container[i].first.z-orig.z;
                float d = x*x + y*y + z*z;
                if (d <= dist_limit*dist_limit) return true;
                return false;
        }
};

template <class T>
class Container3d
{
        public:
                typedef std::pair<CvPoint3D32f, T> node_type;
        protected:
                std::vector<node_type> data;
                std::vector<size_t>    search_space;

        public:
                void Add(const CvPoint3D32f& _pos, const T& _data){
                        data.push_back(node_type(_pos, _data));
                        search_space.push_back(data.size()-1);
                }
                void Clear() {
                        data.clear();
                        search_space.clear();
                }
                void ResetSearchSpace() {
                        search_space.resize(data.size());
                        for (size_t i=0; i<search_space.size(); i++)
                        {
                                search_space[i] = i;
                        }
                }
                void Erase(size_t index) {
                        typename std::vector<node_type>::iterator iter_d;
                        iter_d = data.begin();
                        for (size_t i=0; i<index; i++) iter_d++;
                        data.erase(iter_d);
                        ResetSearchSpace();
                }
                template <typename Compare>
                int Sort(Compare comp) {
                        stable_sort(search_space.begin(), search_space.end(), comp);
                        return search_space.size();
                }

                template <typename Test>
                int Limit(Test test) {
                        std::vector<size_t>::iterator iter;
                        for (iter = search_space.begin(); iter != search_space.end();) {
                                if (!test(*iter)) {
                                        iter = search_space.erase(iter);
                                } else {
                                        iter++;
                                }
                        }
                        return search_space.size();
                }

                class Iterator : public std::iterator<std::forward_iterator_tag, node_type>
                {
                        protected:
                                Container3d<T> *container;
                                std::vector<size_t>::iterator iter;
                        public:
                                Iterator() {}
                                Iterator(Container3d<T> *_container, std::vector<size_t>::iterator _iter) : container(_container), iter(_iter) {}
                                node_type &operator*() const { return container->data[*iter]; }
                                node_type *operator->() const { return &(operator*()); }
                                virtual Iterator& operator++() { ++iter; return *this; }
                                bool operator==(const Iterator& _m) const { return iter == _m.iter; }
                                bool operator!=(const Iterator& _m) const { return iter != _m.iter; }
                                size_t GetIndex() { return *iter; }
                };

                Iterator begin() {
                        return Iterator(this, search_space.begin());
                }

                Iterator end() {
                        return Iterator(this, search_space.end());
                }

                size_t size() const { return data.size(); }

                size_t GetIndex(Iterator &iter) {
                        return iter.GetIndex();
                }

                node_type &operator[](size_t index) {
                        return data[index];
                }

                size_t GetIndex(T *p) {
                        size_t i=0;
                        for (; i<data.size(); ++i)
                        {
                                if (data[i].second.get() == p) break;
                        }
                        return i;
                }
};

} // namespace alvar

#endif