Container3d.h
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #ifndef CONTAINER_3D
00025 #define CONTAINER_3D
00026 
00033 #include "cv.h"
00034 #include <utility>
00035 #include <vector>
00036 #include <algorithm>
00037 
00038 namespace alvar {
00039 
00040 template <class T> class Container3d;
00041 
00043 template <class T>
00044 class Container3dSortDist {
00045 protected:
00046         CvPoint3D32f orig;
00047         Container3d<T> &container;
00048 public:
00049         Container3dSortDist(Container3d<T> &_container, const CvPoint3D32f _orig) : container(_container), orig(_orig) {}
00050         bool operator()(size_t i1, size_t i2)
00051         {
00052                 float x1 = container[i1].first.x-orig.x, x2 = container[i2].first.x-orig.x;
00053                 float y1 = container[i1].first.y-orig.y, y2 = container[i2].first.y-orig.y;
00054                 float z1 = container[i1].first.z-orig.z, z2 = container[i2].first.z-orig.z;
00055                 float d1 = x1*x1 + y1*y1 + z1*z1;
00056                 float d2 = x2*x2 + y2*y2 + z2*z2;
00057                 return d1<d2;
00058         }
00059 };
00060 
00062 template <class T>
00063 class Container3dSortSize {
00064 protected:
00065         Container3d<T> &container;
00066 public:
00067         Container3dSortSize(Container3d<T> &_container) : container(_container) {}
00068         bool operator()(size_t i1, size_t i2)
00069         {
00070                 return (container[i1].second->size() > container[i2].second->size());
00071         }
00072 };
00073 
00075 template <class T>
00076 class Container3dLimitDist {
00077 protected:
00078         Container3d<T> &container;
00079         CvPoint3D32f orig;
00080         float dist_limit;
00081 public:
00082         Container3dLimitDist(Container3d<T> &_container, const CvPoint3D32f _orig, float _dist_limit) 
00083                 : container(_container),orig(_orig),dist_limit(_dist_limit) {}
00084         bool operator()(size_t i) const {
00085                 float x = container[i].first.x-orig.x;
00086                 float y = container[i].first.y-orig.y;
00087                 float z = container[i].first.z-orig.z;
00088                 float d = x*x + y*y + z*z;
00089                 if (d <= dist_limit*dist_limit) return true;
00090                 return false;
00091         }
00092 };
00093 
00165 template <class T>
00166 class Container3d
00167 {
00168         public:
00170                 typedef std::pair<CvPoint3D32f, T> node_type;
00171         protected:
00173                 std::vector<node_type> data;
00175                 std::vector<size_t>    search_space;
00176 
00177         public:
00179                 void Add(const CvPoint3D32f& _pos, const T& _data){
00180                         data.push_back(node_type(_pos, _data));
00181                         search_space.push_back(data.size()-1);
00182                 }
00184                 void Clear() {
00185                         data.clear();
00186                         search_space.clear();
00187                 }
00189                 void ResetSearchSpace() {
00190                         search_space.resize(data.size());
00191                         for (size_t i=0; i<search_space.size(); i++)
00192                         {
00193                                 search_space[i] = i;
00194                         }
00195                 }
00197                 void Erase(size_t index) {
00198                         typename std::vector<node_type>::iterator iter_d;
00199                         iter_d = data.begin();
00200                         for (size_t i=0; i<index; i++) iter_d++;
00201                         data.erase(iter_d);
00202                         ResetSearchSpace();
00203                 }
00205                 template <typename Compare>
00206                 int Sort(Compare comp) {
00207                         stable_sort(search_space.begin(), search_space.end(), comp);
00208                         return search_space.size();
00209                 }
00210 
00212                 template <typename Test>
00213                 int Limit(Test test) {
00214                         std::vector<size_t>::iterator iter;
00215                         for (iter = search_space.begin(); iter != search_space.end();) {
00216                                 if (!test(*iter)) {
00217                                         iter = search_space.erase(iter);
00218                                 } else {
00219                                         iter++;
00220                                 }
00221                         }
00222                         return search_space.size();
00223                 }
00224 
00231                 class Iterator : public std::iterator<std::forward_iterator_tag, node_type>
00232                 {
00233                         protected:
00234                                 Container3d<T> *container;
00235                                 std::vector<size_t>::iterator iter;
00236                         public:
00237                                 Iterator() {}
00238                                 Iterator(Container3d<T> *_container, std::vector<size_t>::iterator _iter) : container(_container), iter(_iter) {}
00239                                 node_type &operator*() const { return container->data[*iter]; }
00240                                 node_type *operator->() const { return &(operator*()); }
00241                                 virtual Iterator& operator++() { ++iter; return *this; }
00242                                 bool operator==(const Iterator& _m) const { return iter == _m.iter; }
00243                                 bool operator!=(const Iterator& _m) const { return iter != _m.iter; }
00244                                 size_t GetIndex() { return *iter; }
00245                 };
00246 
00248                 Iterator begin() {
00249                         return Iterator(this, search_space.begin());
00250                 }
00251 
00253                 Iterator end() {
00254                         return Iterator(this, search_space.end());
00255                 }
00256 
00258                 size_t size() const { return data.size(); }
00259 
00261                 size_t GetIndex(Iterator &iter) {
00262                         return iter.GetIndex();
00263                 }
00264 
00266                 node_type &operator[](size_t index) {
00267                         return data[index];
00268                 }
00269 
00271                 size_t GetIndex(T *p) {
00272                         size_t i=0;
00273                         for (; i<data.size(); ++i)
00274                         {
00275                                 if (data[i].second.get() == p) break;
00276                         }
00277                         return i;
00278                 }
00279 };
00280 
00281 } // namespace alvar
00282 
00283 #endif
00284 


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:15