1 /*******************************************************************************
2 * Copyright 2013-2014 EPFL *
3 * Copyright 2013-2014 Quentin Bonnard *
4 * *
5 * This file is part of chilitags. *
6 * *
7 * Chilitags is free software: you can redistribute it and/or modify *
8 * it under the terms of the Lesser GNU General Public License as *
9 * published by the Free Software Foundation, either version 3 of the *
10 * License, or (at your option) any later version. *
11 * *
12 * Chilitags is distributed in the hope that it will be useful, *
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
15 * GNU Lesser General Public License for more details. *
16 * *
17 * You should have received a copy of the GNU Lesser General Public License *
18 * along with Chilitags. If not, see <http://www.gnu.org/licenses/>. *
19 *******************************************************************************/
20
21 #include "Filter.hpp"
22
23 #include <opencv2/core/core.hpp>
24
25 namespace chilitags {
26
FindOutdated(int persistence)27 FindOutdated::FindOutdated(int persistence) :
28 mPersistence(persistence),
29 mDisappearanceTime()
30 {
31 }
32
operator ()(const std::map<int,Quad> & tags)33 std::vector<int> FindOutdated::operator()(const std::map<int, Quad> &tags){
34
35 std::vector<int> tagsToForget;
36
37 auto tagIt = tags.cbegin();
38 auto ageIt = mDisappearanceTime.begin();
39
40 // for each tags that are detected in the current frame
41 while (tagIt != tags.end()) {
42
43 // update all the tags that come before the current tag,
44 // i.e. that haven't bee detected this time
45 while (ageIt != mDisappearanceTime.end()
46 && ageIt->first < tagIt->first) {
47
48 if (ageIt->second >= mPersistence) {
49 // remove the tags that haven't been seen for too long
50 tagsToForget.push_back(ageIt->first);
51 ageIt = mDisappearanceTime.erase(ageIt);
52 } else {
53 // mark as older the last update of the others
54 ++(ageIt->second);
55 ++ageIt;
56 }
57 }
58
59 ageIt = mDisappearanceTime.insert(ageIt, std::make_pair(tagIt->first, 0));
60
61 ++tagIt;
62 ++ageIt;
63 }
64
65 // update the remaining tags that have not been detected in this frame either
66 while (ageIt != mDisappearanceTime.end()) {
67
68 if (ageIt->second >= mPersistence) {
69 // remove the tags that haven't been seen for too long
70 tagsToForget.push_back(ageIt->first);
71 ageIt = mDisappearanceTime.erase(ageIt);
72 } else {
73 // mark as older the last update of the others
74 ++ageIt->second;
75 ++ageIt;
76 }
77 }
78
79
80 return tagsToForget;
81 }
82
Filter(int persistence,float gain)83 Filter::Filter(int persistence, float gain) :
84 mFindOutdated(persistence),
85 mGain(gain),
86 mFilteredCoordinates()
87 {
88 }
89
operator ()(const std::map<int,Quad> & tags)90 const std::map<int, Quad> & Filter::operator()(
91 const std::map<int, Quad> &tags) {
92
93 for(const auto &tagToForget : mFindOutdated(tags)) {
94 //TODO lookup can be avoided if Ids are sorted
95 mFilteredCoordinates.erase(tagToForget);
96 }
97
98 const float gainComplement = 1.0f - mGain;
99
100 auto filteredIt = mFilteredCoordinates.begin();
101 for (const auto &tag : tags) {
102 while (filteredIt != mFilteredCoordinates.end()
103 && filteredIt->first < tag.first) {
104 ++filteredIt;
105 }
106
107 if (filteredIt != mFilteredCoordinates.end()
108 && filteredIt->first == tag.first) {
109 cv::addWeighted(filteredIt->second, mGain,
110 tag.second, gainComplement,
111 0.0f, filteredIt->second);
112 }
113 else {
114 filteredIt = mFilteredCoordinates.insert(filteredIt, tag);
115 }
116 }
117
118 return mFilteredCoordinates;
119 }
120
121 }
122